#!/usr/bin/python3
import cv2
import numpy as np
import time
import threading
import math
import Serial_Servo_Running as SSR
import signal
import PWMServo

#################################################参数设置####################################################

ori_width = int(4 * 160)  # 原始图像640x480
ori_height = int(3 * 160)
r_width = int(4 * 20)  # 处理图像时缩小为80x60,加快处理速度，谨慎修改！
r_height = int(3 * 20)
# 张中汉所用颜色字典
color_range = {'yellow_door': [(10, 43, 46), (34, 255, 255)],
               'red_floor1': [(0, 43, 46), (10, 255, 255)],
               'red_floor2': [(156, 43, 46), (180, 255, 255)],
               'green_bridge': [(35, 43, 20), (100, 255, 255)],
               'yellow_hole': [(10, 70, 46), (34, 255, 255)],
               'black_hole': [(0, 0, 0), (180, 255, 80)],
               'black_gap': [(0, 0, 0), (180, 255, 100)],
               'black_dir': [(0, 0, 0), (180, 255, 46)],
               'blue': [(110, 43, 46), (124, 255, 255)],
               'black_door': [(0, 0, 0), (180, 255, 46)],
               }
# 姜颜曹所用颜色字典
color_dist = {'red': {'Lower': np.array([0, 60, 60]), 'Upper': np.array([6, 255, 255])},
              'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])},
              'green': {'Lower': np.array([35, 43, 46]), 'Upper': np.array([77, 255, 255])},
              'black': {'Lower': np.array([0, 0, 0]), 'Upper': np.array([180, 255, 46])},
              'black_obscle': {'Lower': np.array([0, 0, 0]), 'Upper': np.array([180, 255, 60])},
              'black_dir': {'Lower': np.array([0, 0, 0]), 'Upper': np.array([180, 255, 46])}
              }

#################################################初始化#########################################################
stream = "http://127.0.0.1:8080/?action=stream?dummy=param.mjpg"
cap = cv2.VideoCapture(stream)
Running = False
#Running = True
debug = True
state = 1
step = 0
state_sel = None
#state_sel = 'hole'
reset = 0
skip = 0
PWMServo.setServo(1, 1900, 500)  # NO3机器人，初始云台角度
PWMServo.setServo(2, 1510, 500)
SSR.running_action_group('0', 1)
ret = False  # 读取图像标志位
org_img = None  # 全局变量，原始图像


################################################读取图像线程#################################################
def get_img():
    global org_img
    global ret
    global Running
    global cap
    while True:
        if cap.isOpened():
            ret, org_img = cap.read()
        else:
            time.sleep(0.01)


# 读取图像线程
th1 = threading.Thread(target=get_img)
th1.setDaemon(True)  # 设置为后台线程，这里默认是False，设置为True之后则主线程不用等待子线程
th1.start()


################################################按键控制线程###################################################
# 暂停信号的回调
def cv_stop(signum, frame):  # key2按下，暂停程序
    global skip
    print("跳至下一关卡")
    skip = 1
    


# 继续信号的回调
def cv_continue(signum, frame):
    global step, Running, state, reset
    if Running is False:  # key1按下，启动程序
        print("开始")
        SSR.running_action_group('00', 1)
        state = 1
        Running = True
    elif Running is True:  # key1按下，从这一关卡的起点开始
        print('当前关卡重置')
        reset = 1
        step = 0
        # SSR.running_action_group('0', 1)


#   注册信号回调
signal.signal(signal.SIGTSTP, cv_stop)
signal.signal(signal.SIGCONT, cv_continue)


#########################################一些子函数##################################################################

# 映射函数，缩小后的图片处理后得到的坐标，再放大得到原图所对应的点
def leMap(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

# 得到最大轮廓和对应的最大面积 张中汉所用
def getAreaMaxContour(contours):
    contour_area_temp = 0
    contour_area_max = 0
    area_max_contour = None
    #area_max_contour = np.array([[[0, 0]], [[0, 1]], [[1, 1]], [[1, 0]]])
    for c in contours:  # 历遍所有轮廓
        contour_area_temp = math.fabs(cv2.contourArea(c))  # 计算轮廓面积
        if contour_area_temp > contour_area_max:
            contour_area_max = contour_area_temp
            if contour_area_temp > 25:  #只有在面积大于25时，最大面积的轮廓才是有效的，以过滤干扰
                area_max_contour = c
    return area_max_contour, contour_area_max  # 返回最大的轮廓

########得到最大轮廓 姜颜曹所用############
def getAreaMaxContour2(contours, area=1):
    contour_area_max = 0
    area_max_contour = None
    for c in contours:
        contour_area_temp = math.fabs(cv2.contourArea(c))
        if contour_area_temp > contour_area_max:
            contour_area_max = contour_area_temp
            if contour_area_temp > area:  # 面积大于1
                area_max_contour = c
    return area_max_contour
######### 得到所有轮廓的面积##########
def getAreaSumContour(contours):
    contour_area_sum = 0
    for c in contours:  # 历遍所有轮廓
        contour_area_sum += math.fabs(cv2.contourArea(c))  # 计算轮廓面积
    return contour_area_sum  # 返回最大的面积
###############得到线形的总的轮廓###############
def getLineSumContour(contours, area=1):
    contours_sum = None
    for c in contours:
        area_temp = math.fabs(cv2.contourArea(c))
        rect = cv2.minAreaRect(c)#最小外接矩形
        box = np.int0(cv2.boxPoints(rect))#最小外接矩形的四个顶点
        edge1=math.sqrt(math.pow(box[3, 1] - box[0, 1], 2) + math.pow(box[3, 0] - box[0, 0], 2))
        edge2=math.sqrt(math.pow(box[3, 1] - box[2, 1], 2) + math.pow(box[3, 0] - box[2, 0], 2))
        ratio=edge1/edge2
        if (area_temp > area) and (ratio>3 or ratio<0.33):
            contours_sum = c
            break
    for c in contours:
        area_temp = math.fabs(cv2.contourArea(c))
        rect = cv2.minAreaRect(c)#最小外接矩形
        box = np.int0(cv2.boxPoints(rect))#最小外接矩形的四个顶点
        edge1=math.sqrt(math.pow(box[3, 1] - box[0, 1], 2) + math.pow(box[3, 0] - box[0, 0], 2))
        edge2=math.sqrt(math.pow(box[3, 1] - box[2, 1], 2) + math.pow(box[3, 0] - box[2, 0], 2))
        ratio=edge1/edge2
        #print(ratio)
        if (area_temp > area) and (ratio>3 or ratio<0.33):
            contours_sum = np.concatenate((contours_sum, c), axis=0)  # 将所有轮廓点拼接到一起          
    return contours_sum
###################得到全部的总的轮廓##############
def getSumContour(contours, area=1):
    contours_sum = None
    for c in contours:
        area_temp = math.fabs(cv2.contourArea(c))
        if (area_temp > area):
            contours_sum = c
            break
    for c in contours:
        area_temp = math.fabs(cv2.contourArea(c))
        if (area_temp > area):
            contours_sum = np.concatenate((contours_sum, c), axis=0)  # 将所有轮廓点拼接到一起          
    return contours_sum
################得到gap-direction的轮廓，去掉斜率为正##############
def getgapContour(contours,resize_width,resize_height,area=1):
    contours_sum = None
    for c in contours:
        area_temp = math.fabs(cv2.contourArea(c))
        rect = cv2.minAreaRect(c)#最小外接矩形
        center, w_h, angle = rect  # 中心点 宽高 旋转角度
        box = np.int0(cv2.boxPoints(rect))#最小外接矩形的四个顶点
        # 求得矩形的旋转角度，if条件是为了判断长的一条边的旋转角度，因为box存储的点的顺序不确定\
        if math.sqrt(math.pow(box[3, 1] - box[0, 1], 2) + math.pow(box[3, 0] - box[0, 0], 2)) > math.sqrt(math.pow(box[3, 1] - box[2, 1], 2) + math.pow(box[3, 0] - box[2, 0], 2)):
            if box[3, 0] == box[0, 0]:
                angle = 90
            else:
                angle = - math.atan((box[3, 1] - box[0, 1]) / (box[3, 0] - box[0, 0])) * 180.0 / math.pi
        else:
            if box[3, 0] == box[2, 0]:
                angle = 90
            else:
                angle = - math.atan((box[3, 1] - box[2, 1]) / (box[3, 0] - box[2, 0])) * 180.0 / math.pi  # 负号是因为坐标原点的问题
        if center[0] < resize_width/3 and center[0] < resize_height/3 and angle >=0:
            continue
        elif (area_temp > area):
            contours_sum = c
            break
    for c in contours:
        area_temp = math.fabs(cv2.contourArea(c))
        rect = cv2.minAreaRect(c)#最小外接矩形
        center, w_h, angle = rect  # 中心点 宽高 旋转角度
        box = np.int0(cv2.boxPoints(rect))#最小外接矩形的四个顶点
        # 求得矩形的旋转角度，if条件是为了判断长的一条边的旋转角度，因为box存储的点的顺序不确定\
        if math.sqrt(math.pow(box[3, 1] - box[0, 1], 2) + math.pow(box[3, 0] - box[0, 0], 2)) > math.sqrt(math.pow(box[3, 1] - box[2, 1], 2) + math.pow(box[3, 0] - box[2, 0], 2)):
            if box[3, 0] == box[0, 0]:
                angle = 90
            else:
                angle = - math.atan((box[3, 1] - box[0, 1]) / (box[3, 0] - box[0, 0])) * 180.0 / math.pi
        else:
            if box[3, 0] == box[2, 0]:
                angle = 90
            else:
                angle = - math.atan((box[3, 1] - box[2, 1]) / (box[3, 0] - box[2, 0])) * 180.0 / math.pi  # 负号是因为坐标原点的问题
        if center[0] < resize_width/3 and center[0] < resize_height/3 and angle >=0:
            #print(center[0])
            #print(center[1])
            #print(angle)
            continue
        elif (area_temp > area):
            contours_sum = np.concatenate((contours_sum, c), axis=0)  # 将所有轮廓点拼接到一起          
    return contours_sum
################看右边黑线调整方向和位置##########
def direction(resize_width, resize_height, sel):  # sel == 1:排除地雷  sel == 2:排除外界干扰  sel=3:gap
    global reset, skip, org_img
    dis_flag = False
    angle_flag = False
    angle = 90
    dis = 0
    step_dir = 0
    notok = True
    see = False
    PWMServo.setServo(1, 1800, 500)
    PWMServo.setServo(2, 850, 500)
    # PWMServo.setServo(1, 2090, 500)
    # PWMServo.setServo(2, 1430, 500)
    time.sleep(0.5)
    SSR.running_action_group("00", 1)  # stand
    while True:
        if reset == 1 or skip == 1:  # 跳关或者重置
            cv2.destroyAllWindows()
            break
        # t1 = cv2.getTickCount()
        OrgFrame = org_img
        frame = cv2.resize(OrgFrame, (resize_width, resize_height), interpolation=cv2.INTER_LINEAR)
        cv2.imshow('init', frame)
        # 获取图像中心点坐标x, y
        center = []
        # 开始处理图像
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        hsv = cv2.GaussianBlur(hsv, (3, 3), 0)
        #Imask = cv2.inRange(hsv, color_range['black_dir'][0], color_range['black_dir'][1])
        Imask = cv2.inRange(hsv, color_dist['black_dir']['Lower'], color_dist['black_dir']['Upper'])
        Imask = cv2.erode(Imask, None, iterations=2)
        Imask = cv2.dilate(Imask, np.ones((3, 3), np.uint8), iterations=2)
        cv2.imshow('black', Imask)
        _, cnts, hierarchy = cv2.findContours(Imask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_L1)  # 找出所有轮廓
        if sel == 1:
            cnt_sum = getLineSumContour(cnts, area=100)
        elif sel == 2:
            cnt_sum = getSumContour(cnts, area=100)
        elif sel == 3:
            cnt_sum = getgapContour(cnts, resize_width, resize_height, area=100)
        else:
            cnt_sum = None
        cv2.drawContours(frame, cnt_sum, -1, (255, 0, 255), 3)
        if cnt_sum is not None:
            see = True
            rect = cv2.minAreaRect(cnt_sum)  # 最小外接矩形
            box = np.int0(cv2.boxPoints(rect))  # 最小外接矩形的四个顶点
            cv2.drawContours(frame, [box], -1, [0, 0, 255, 255], 3)
            if math.sqrt(math.pow(box[3, 1] - box[0, 1], 2) + math.pow(box[3, 0] - box[0, 0], 2)) > math.sqrt(
                    math.pow(box[3, 1] - box[2, 1], 2) + math.pow(box[3, 0] - box[2, 0], 2)):
                if box[3, 0] - box[0, 0] == 0:
                    angle = 90
                else:
                    angle = - math.atan((box[3, 1] - box[0, 1]) / (box[3, 0] - box[0, 0])) * 180.0 / math.pi
                if box[3, 1] + box[0, 1] > box[2, 1] + box[1, 1]:
                    edge_y1, edge_y2 = box[3, 1], box[0, 1]
                    dis = int((edge_y1 + edge_y2) / 2)
                    cv2.circle(frame, (int((box[3, 0] + box[0, 0]) / 2), int((box[3, 1] + box[0, 1]) / 2)), 10,
                               (0, 0, 255), -1)  # 画出中心点
                else:
                    edge_y1, edge_y2 = box[2, 1], box[1, 1]
                    dis = int((edge_y1 + edge_y2) / 2)
                    cv2.circle(frame, (int((box[2, 0] + box[1, 0]) / 2), int((box[2, 1] + box[1, 1]) / 2)), 10,
                               (0, 0, 255), -1)  # 画出中心点
            else:
                if box[3, 0] - box[2, 0] == 0:
                    angle = 90
                else:
                    angle = - math.atan(
                        (box[3, 1] - box[2, 1]) / (box[3, 0] - box[2, 0])) * 180.0 / math.pi  # 负号是因为坐标原点的问题
                if box[3, 1] + box[2, 1] > box[0, 1] + box[1, 1]:
                    edge_y1, edge_y2 = box[3, 1], box[2, 1]
                    dis = int((edge_y1 + edge_y2) / 2)
                    cv2.circle(frame, (int((box[3, 0] + box[2, 0]) / 2), int((box[3, 1] + box[2, 1]) / 2)), 10,
                               (0, 0, 255), -1)  # 画出中心点
                else:
                    edge_y1, edge_y2 = box[0, 1], box[1, 1]
                    dis = int((edge_y1 + edge_y2) / 2)
                    cv2.circle(frame, (int((box[0, 0] + box[1, 0]) / 2), int((box[0, 1] + box[1, 1]) / 2)), 10,
                               (0, 0, 255), -1)  # 画出中心点
        else:
            see = False
        if dis_flag and angle_flag:
            notok = False

        cv2.putText(frame, "angle:" + str(angle),
                    (10, frame.shape[0] - 35), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
        cv2.putText(frame, "distance:" + str(dis),
                    (10, frame.shape[0] - 55), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
        cv2.putText(frame, "notok:" + str(notok),
                    (10, frame.shape[0] - 75), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
        cv2.imshow('frame', frame)
        cv2.waitKey(1)
        # t2 = cv2.getTickCount()
        # time_r = (t2 - t1) / cv2.getTickFrequency()
        # fps = 1.0 / time_r
        # print(fps)
        if step_dir == 0:
            if not see:  # not see the edge
                SSR.running_action_group("161", 1)  # 右move小
                time.sleep(0.2)
            else:
                SSR.running_action_group("00", 1)  # 右move小
                step_dir = 1
        elif step_dir == 1:
            if notok:
                if not see:  # not see the edge
                    SSR.running_action_group("203", 1)  # 右move小
                    time.sleep(0.2)
                elif angle_flag:
                    angle_flag = True
                    if dis < 180:
                        SSR.running_action_group("161", 1)  # 右移两步
                    elif dis > 300:
                        SSR.running_action_group("160", 1)  # 左移两步
                    else:
                        dis_flag = True
                else:
                    if dis < 80:
                        if angle > -13:
                            SSR.running_action_group("202", 1)  # 左转小
                        elif angle < -16:
                            SSR.running_action_group("203", 1)  # 右转小
                        else:
                            angle_flag = True
                    elif dis > 400:
                        if angle > -24:
                            SSR.running_action_group("202", 1)  # 左转小
                        elif angle < -28:
                            SSR.running_action_group("203", 1)  # 右转小
                        else:
                            angle_flag = True
                    else:
                        if angle > -18:
                            SSR.running_action_group("202", 1)  # 左转小
                        elif angle < -22:
                            SSR.running_action_group("203", 1)  # 右转小
                        else:
                            angle_flag = True
                    time.sleep(0.2)
            else:
                cv2.destroyAllWindows()
                SSR.running_action_group("00", 1)  # 静止
                break
def direction_only_angle(resize_width, resize_height):
    global org_img, reset, skip
    angle_flag = False
    angle = 90
    dis = 0
    see = False
    PWMServo.setServo(1, 1800, 500)
    PWMServo.setServo(2, 850, 500)
    time.sleep(0.5)
    SSR.running_action_group("00", 1)  # stand
    while True:
        if reset == 1 or skip == 1:  # 跳关或者重置
            cv2.destroyAllWindows()
            break
        OrgFrame = org_img
        frame = cv2.resize(OrgFrame, (resize_width, resize_height), interpolation=cv2.INTER_LINEAR)
        #cv2.imshow('init', frame)
        # 获取图像中心点坐标x, y
        center = []
        # 开始处理图像
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        hsv = cv2.GaussianBlur(hsv, (3, 3), 0)
        Imask = cv2.inRange(hsv, color_dist['black_dir']['Lower'], color_dist['black_dir']['Upper'])
        Imask = cv2.erode(Imask, None, iterations=2)
        Imask = cv2.dilate(Imask, np.ones((3, 3), np.uint8), iterations=2)
        cv2.imshow('black', Imask)
        _, cnts, hierarchy = cv2.findContours(Imask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_L1)  # 找出所有轮廓
        cnt_sum = getLineSumContour(cnts, area=1)
        cv2.drawContours(frame, cnt_sum, -1, (255, 0, 255), 3)
        if cnt_sum is not None:
            see = True
            rect = cv2.minAreaRect(cnt_sum)#最小外接矩形
            box = np.int0(cv2.boxPoints(rect))#最小外接矩形的四个顶点
            if math.sqrt(math.pow(box[3, 1] - box[0, 1], 2) + math.pow(box[3, 0] - box[0, 0], 2)) > math.sqrt(math.pow(box[3, 1] - box[2, 1], 2) + math.pow(box[3, 0] - box[2, 0], 2)):
                if box[3, 0] - box[0, 0]==0:
                    angle=90
                else:
                    angle = - math.atan((box[3, 1] - box[0, 1]) / (box[3, 0] - box[0, 0]))*180.0/math.pi
                if box[3,1]+box[0,1]>box[2,1]+box[1,1]:
                    edge_y1,edge_y2=box[3,1],box[0,1]
                    dis=int((edge_y1+edge_y2)/2)
                    cv2.circle(OrgFrame, (int((box[3,0]+box[0,0])/2), int((box[3,1]+box[0,1])/2)), 10, (0,0,255), -1)#画出中心点
                else:
                    edge_y1,edge_y2=box[2,1],box[1,1]
                    dis=int((edge_y1+edge_y2)/2)
                    cv2.circle(OrgFrame, (int((box[2,0]+box[1,0])/2), int((box[2,1]+box[1,1])/2)), 10, (0,0,255), -1)#画出中心点
            else:
                if box[3, 0] - box[2, 0]==0:
                    angle=90
                else:
                    angle = - math.atan((box[3, 1] - box[2, 1]) / (box[3, 0] - box[2, 0]))*180.0/math.pi # 负号是因为坐标原点的问题
                if box[3,1]+box[2,1]>box[0,1]+box[1,1]:
                    edge_y1,edge_y2=box[3,1],box[2,1]
                    dis=int((edge_y1+edge_y2)/2)
                    cv2.circle(OrgFrame, (int((box[3,0]+box[2,0])/2), int((box[3,1]+box[2,1])/2)), 10, (0,0,255), -1)#画出中心点
                else:
                    edge_y1,edge_y2=box[0,1],box[1,1]
                    dis=int((edge_y1+edge_y2)/2)
                    cv2.circle(OrgFrame, (int((box[0,0]+box[1,0])/2), int((box[0,1]+box[1,1])/2)), 10, (0,0,255), -1)#画出中心点
        else:
            see = False

        cv2.putText(frame, "angle:" + str(angle),
                    (10, frame.shape[0] - 35), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)

        cv2.imshow('frame', frame)
        cv2.waitKey(1)

        if not see:  # not see the edge
            cv2.destroyAllWindows()
            break
        else:
            if dis < 80:
                if angle > -13:
                    SSR.running_action_group("202", 1)  # 左转小
                elif angle < -16:
                    SSR.running_action_group("203", 1)  # 右转小
                else:
                    break
            elif dis > 400:
                if angle > -24:
                    SSR.running_action_group("202", 1)  # 左转小
                elif angle < -28:
                    SSR.running_action_group("203", 1)  # 右转小
                else:
                    break
            else:
                if angle > -18:
                    SSR.running_action_group("202", 1)  # 左转小
                elif angle < -22:
                    SSR.running_action_group("203", 1)  # 右转小
                else:
                    break
            time.sleep(0.2)

########判断三个关卡的随机顺序#########
def state_choose(r_w, r_h):
    global org_img
    s_sel = None
    org_img_copy = cv2.resize(org_img, (r_w, r_h), interpolation=cv2.INTER_CUBIC)  # 将图片缩放
    frame_gauss = cv2.GaussianBlur(org_img_copy, (3, 3), 0)  # 高斯模糊
    frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
    frame_floor = cv2.inRange(frame_hsv, color_range['red_floor1'][0],
                              color_range['red_floor1'][1])  # 对原图像和掩模(颜色的字典)进行位运算
    frame_bridge = cv2.inRange(frame_hsv, color_range['green_bridge'][0],
                               color_range['green_bridge'][1])  # 对原图像和掩模(颜色的字典)进行位运算
    frame_hole = cv2.inRange(frame_hsv, color_range['yellow_hole'][0],
                             color_range['yellow_hole'][1])  # 对原图像和掩模(颜色的字典)进行位运算
    (image, contours_floor, hierarchy) = cv2.findContours(frame_floor, cv2.RETR_EXTERNAL,
                                                          cv2.CHAIN_APPROX_NONE)  # 找出红色台阶轮廓
    (image, contours_bridge, hierarchy) = cv2.findContours(frame_bridge, cv2.RETR_EXTERNAL,
                                                           cv2.CHAIN_APPROX_NONE)  # 找出绿色独木桥轮廓
    (image, contours_hole, hierarchy) = cv2.findContours(frame_hole, cv2.RETR_EXTERNAL,
                                                         cv2.CHAIN_APPROX_NONE)  # 找出黄色方坑轮廓
    areaMaxContour, area_max_floor = getAreaMaxContour(contours_floor)  # 找出最大轮廓
    percent_floor = round(area_max_floor * 100 / (r_w * r_h), 2)  # 最大轮廓百分比
    areaMaxContour, area_max_bridge = getAreaMaxContour(contours_bridge)  # 找出最大轮廓
    percent_bridge = round(area_max_bridge * 100 / (r_w * r_h), 2)  # 最大轮廓百分比
    areaMaxContour, area_max_hole = getAreaMaxContour(contours_hole)  # 找出最大轮廓
    percent_hole = round(area_max_hole * 100 / (r_w * r_h), 2)  # 最大轮廓百分比
    #cv2.imshow('org_img_copy', org_img_copy)  # 显示图像
    #cv2.waitKey(1)
    if percent_floor > percent_bridge and percent_floor > percent_hole and percent_floor > 5:
        s_sel = 'floor'
    elif percent_bridge > percent_floor and percent_bridge > percent_hole and percent_bridge > 5:
        s_sel = 'bridge'
    elif percent_hole > percent_floor and percent_hole > percent_bridge and percent_hole > 5:
        s_sel = 'hole'
    return s_sel



################################################第一关：起点#############################################
def start_door(r_w, r_h):
    global org_img, state, state_sel, step, reset, skip, debug
    if state == 1:  # 初始化
        print('进入start_door')
        step = 0
        PWMServo.setServo(1, 1900, 500)  # NO3机器人，初始云台角度
        PWMServo.setServo(2, 1510, 500)
        time.sleep(1)
        #cv2.destroyAllWindows()
        #SSR.running_action_group('0', 1)
        # time.sleep(0.5) #等待摄像头稳定
    else:
        return
    while state == 1:
        if reset == 1:  # 是否为重置情况
            cv2.destroyAllWindows()
            PWMServo.setServo(1, 1900, 500)  # NO3机器人，初始云台角度
            PWMServo.setServo(2, 1510, 500)
            reset = 0
            step = 0
            SSR.running_action_group('0', 1)
        if skip == 1:  # 跳至下一关
            cv2.destroyAllWindows()
            state += 1
            skip = 0
            state_sel = None
            SSR.running_action_group('666', 1)
            time.sleep(5)
            break
        if step == 0: #判断门是否抬起
            t1 = cv2.getTickCount()
            border = cv2.copyMakeBorder(org_img, 12, 12, 16, 16, borderType=cv2.BORDER_CONSTANT,
                                        value=(255, 255, 255))  # 扩展白边，防止边界无法识别
            org_img_copy = cv2.resize(border, (r_w, r_h), interpolation=cv2.INTER_CUBIC)  # 将图片缩放
            frame_gauss = cv2.GaussianBlur(org_img_copy, (3, 3), 0)  # 高斯模糊
            frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
            #h, s, v = cv2.split(frame_hsv)  # 分离出各个HSV通道
            #v = cv2.equalizeHist(v)  # 直方图化
            #Frame = cv2.merge((h, s, v))  # 合并三个通道
            frame_door1 = cv2.inRange(frame_hsv, color_range['yellow_door'][0],
                                       color_range['yellow_door'][1])  # 对原图像和掩模(颜色的字典)进行位运算
            frame_door2 = cv2.inRange(frame_hsv, color_range['black_hole'][0],
                                       color_range['black_hole'][1])  # 对原图像和掩模(颜色的字典)进行位运算
            frame_door = cv2.add(frame_door1, frame_door2)                 
            opened = cv2.morphologyEx(frame_door, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))  # 开运算 去噪点
            closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8))  # 闭运算 封闭连接
            # print(closed)

            (image, contours, hierarchy) = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)  # 找出轮廓
            areaMaxContour, area_max = getAreaMaxContour(contours)  # 找出最大轮廓
            percent = round(100 * area_max / (r_w * r_h), 2)  # 最大轮廓的百分比
            if areaMaxContour is not None:
                rect = cv2.minAreaRect(areaMaxContour)  # 矩形框选
                box = np.int0(cv2.boxPoints(rect))  # 点的坐标
                if debug:
                    cv2.drawContours(org_img_copy, [box], 0, (153, 200, 0), 2)  # 将最小外接矩形画在图上
            if debug:
                cv2.imshow('closed', closed)  # 显示图像
                cv2.putText(org_img_copy, 'area: ' + str(percent) + '%', (10, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
                t2 = cv2.getTickCount()
                time_r = (t2 - t1) / cv2.getTickFrequency()
                fps = 1.0 / time_r
                cv2.putText(org_img_copy, "fps:" + str(int(fps)), (30, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
                # cv2.moveWindow('orgFrame', img_center_x, 100)  # 显示框位置
                cv2.imshow('org_img_copy', org_img_copy)  # 显示图像
                cv2.waitKey(1)
            # 根据比例得到是否前进的信息
            
            if percent > 10:
                print(percent)
                time.sleep(0.01)
            else:
                print(percent)
    
                SSR.running_action_group('239', 6)  # 直行6步
                #SSR.running_action_group('157', 1)  # 直行后立正
                step = 1
                PWMServo.setServo(1, 2000, 500)  # NO3机器人，初始云台角度
                PWMServo.setServo(2, 1510, 500)
                cv2.destroyAllWindows()
                #time.sleep(0.5)
                
        elif step == 1:  # 寻找下一关卡
            state_sel = state_choose(160, 120)
            if state_sel is not None:
                print('state_sel: %s' % state_sel)
                #state_sel = None
                cv2.destroyAllWindows()
                SSR.running_action_group('157', 1)  # 直行后立正
                state = 2
                step = 0
                #SSR.running_action_group('157', 1)  # 直行后立正
                break
            else:
                SSR.running_action_group('239', 1)  # 直行1步


#################################################第二关：台阶##########################################
def floor(r_w, r_h):
    global org_img, state, state_sel, step, reset, skip, debug
    if (state == 2 or state == 6 or state == 8) and state_sel == 'floor':  # 初始化
        print('进入floor')
        step = 0
        #PWMServo.setServo(1, 1650, 500) # NO3机器人，初始云台角度
        #PWMServo.setServo(2, 1510, 500)
        PWMServo.setServo(1, 2000, 500) # NO3机器人，初始云台角度
        PWMServo.setServo(2, 1510, 500)
        time.sleep(0.2)
        #SSR.running_action_group('00', 1)
        # time.sleep(0.5) #等待摄像头稳定
    elif (state == 2 or state == 6 or state == 8) and state_sel is None:  # 跳关情况
        state_sel = state_choose(320, 240)
        return
    else:
        return
    while (state == 2 or state == 6 or state == 8) and state_sel == 'floor':
        if reset == 1:  # 是否为重置情况
            cv2.destroyAllWindows()
            PWMServo.setServo(1, 2000, 500)  # NO3机器人，初始云台角度
            PWMServo.setServo(2, 1510, 500)
            reset = 0
            step = 0
            SSR.running_action_group('0', 1)
        elif skip == 1:  # 跳至下一关
            cv2.destroyAllWindows()
            state += 1
            skip = 0
            state_sel = None
            SSR.running_action_group('666', 1)
            time.sleep(5)
            break
        t1 = cv2.getTickCount()
        border = cv2.copyMakeBorder(org_img, 12, 12, 16, 16, borderType=cv2.BORDER_CONSTANT,
                                    value=(0, 0, 0))  # 扩展hei边，防止边界无法识别
        org_img_copy = cv2.resize(border, (r_w, r_h), interpolation=cv2.INTER_CUBIC)  # 将图片缩放
        frame_gauss = cv2.GaussianBlur(org_img_copy, (3, 3), 0)  # 高斯模糊
        frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        frame_red1 = cv2.inRange(frame_hsv, color_range['red_floor1'][0], color_range['red_floor1'][1])  # 对原图像和掩模(颜色的字典)进行位运算
        frame_red2 = cv2.inRange(frame_hsv, color_range['red_floor2'][0],
                                 color_range['red_floor2'][1])  # 对原图像和掩模(颜色的字典)进行位运算
        frame_red = cv2.add(frame_red1, frame_red2)
        opened = cv2.morphologyEx(frame_red, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))  # 开运算 去噪点
        closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8))  # 闭运算 封闭连接
        # print(closed)


        (image, contours, hierarchy) = cv2.findContours(closed, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE)  # 找出轮廓

        areaMaxContour, area_max = getAreaMaxContour(contours)  # 找出最大轮廓
        percent = round(area_max * 100 / (r_w * r_h), 2)  # 最大轮廓百分比
        if debug:
            cv2.imshow('closed', closed)  # 显示图像
            cv2.drawContours(org_img_copy, contours, -1, (255, 0, 255), 1)  # 画出轮廓
            cv2.putText(org_img_copy, 'area:' + str(percent) + '%', (30, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0), 2)
        if areaMaxContour is not None:
            rect = cv2.minAreaRect(areaMaxContour)
            # center, w_h, angle = rect  # 中心点 宽高 旋转角度
            box = np.int0(cv2.boxPoints(rect))  # 点的坐标
            top_right = areaMaxContour[0][0]  # 右上角
            top_left = areaMaxContour[0][0]  # 左上角
            for c in areaMaxContour:
                if c[0][0] + 1.5 * c[0][1] < top_left[0] + 1.5 * top_left[1]:
                    top_left = c[0]
                if (r_w - c[0][0]) + 1.5 * c[0][1] < (r_w - top_right[0]) + 1.5 * top_right[1]:
                    top_right = c[0]
            angle = - math.atan((top_right[1] - top_left[1]) / (top_right[0] - top_left[0])) * 180.0 / math.pi  # 得到角度
            center_x = (top_right[0] + top_left[0]) / 2  # 得到中心坐标
            center_y = (top_right[1] + top_left[1]) / 2
            if debug:
                cv2.drawContours(org_img_copy, [box], 0, (0, 0, 255), 2)  # 将大矩形画在图上
                cv2.circle(org_img_copy, (top_right[0], top_right[1]), 5, [0, 255, 255], 2)
                cv2.circle(org_img_copy, (top_left[0], top_left[1]), 5, [0, 255, 255], 2)
                cv2.putText(org_img_copy, "angle:" + str(angle), (30, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                            2)  # (0, 0, 255)BGR
                cv2.putText(org_img_copy, "center_x:" + str(center_x), (30, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                            2)  # (0, 0, 255)BGR
                cv2.putText(org_img_copy, "center_y:" + str(int(center_y)), (230, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.65,
                            (0, 0, 0), 2)  # (0, 0, 255)BGR
        else:
            angle = 0
            center_x = 0.5 * r_w
            center_y = 0
        if debug:
            t2 = cv2.getTickCount()
            time_r = (t2 - t1) / cv2.getTickFrequency()
            fps = 1.0 / time_r
            # print('fps: %f' %(fps))
            # print('angle: %f' %(angle))
            # print('center_x: %f' %(center_x))
            # print('center_y: %f' %(center_y))
            cv2.putText(org_img_copy, "step:" + str(step), (30, 250), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                        2)  # (0, 0, 255)BGR
            cv2.putText(org_img_copy, "fps:" + str(int(fps)), (30, 300), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                        2)  # (0, 0, 255)BGR
            # cv2.moveWindow('orgFrame', img_center_x, 100)  # 显示框位置
            cv2.imshow('org_img_copy', org_img_copy)  # 显示图像
            cv2.waitKey(1)
        if step == 0:
            SSR.running_action_group('239', 1)  # 到达台阶前
            if percent > 70:  # 接近台阶
                #print('percent > 70')
                SSR.running_action_group('157', 1)  # 93立正
                SSR.running_action_group('232', 2)  # 小步贴近边沿
                SSR.running_action_group('237', 1)  # 更小步贴近边沿
                SSR.running_action_group('226', 1)  # 上台阶
                PWMServo.setServo(1, 1650, 500)  # 抬头
                PWMServo.setServo(2, 1510, 500)  # NO3
                step = 1
                SSR.running_action_group('00', 1)  # 短立正
                time.sleep(0.2)  # 等待摄像头稳定
        elif step == 1:  # 调整方向和位置
            # print('angle: %f' %(angle))
            # print('center_x: %f' %(center_x))
            # print('center_y: %f' %(center_y))
            if angle > 5:  # 需要左转
                SSR.running_action_group('202', 1)
            elif angle < -1:  # 需要右转
                SSR.running_action_group('203', 1)
            elif -1 <= angle <= 5:  # 角度正确
                if center_x > 0.52 * r_w:  # 需要右移
                    SSR.running_action_group('161', 1)
                elif center_x < 0.48 * r_w:  # 需要左移
                    SSR.running_action_group('160', 1)
                elif 0.48 * r_w <= center_x <= 0.52 * r_w:  # 位置正确
                    SSR.running_action_group('239', 10)  # 直行十步
                    SSR.running_action_group('157', 1)
                    PWMServo.setServo(1, 2200, 500)  # 低头，准备看边沿
                    PWMServo.setServo(2, 1510, 500)
                    step = 2
                    time.sleep(0.6)
        elif step == 2:  # 接近下台阶边沿，调整方向
            # print('angle: %f' %(angle))
            # print('center_x: %f' %(center_x))
            # print('center_y: %f' %(center_y))
            if center_y < 0.5 * r_h: #or percent < 10:  # 台阶边沿还远 or  percent<10(由于全红时的识别bug)
                SSR.running_action_group('232', 1)
            elif 0.5 * r_h <= center_y:  # 已经很接近边沿，调整一次方向
                if angle > 7:  # 需要左转
                    SSR.running_action_group('202', 1)
                elif angle < -3:  # 需要右转
                    SSR.running_action_group('203', 1)
                elif -3 <= angle <= 7:
                    step = 3
        elif step == 3:
            if center_y < 0.73 * r_h:
                SSR.running_action_group('237', 1)  # 更小步挪
            else:  # 到达下台阶边沿
                SSR.running_action_group('225', 1)  # 下台阶
                state += 1
                state_sel = None
                step = -1  # 雷阵调方向
                cv2.destroyAllWindows()
                # print('state=3')
                break

#################################################第三关：雷阵#############################################
roi = [(0, 80, 0, 70), (0, 80, 70, 140), (0, 80, 140, 210), (0, 80, 210, 280), (0, 80, 280, 360), (0, 80, 360, 430),
       (0, 80, 430, 500),
       (0, 80, 500, 570), (0, 80, 570, 640), (80, 160, 0, 70), (80, 160, 70, 140), (80, 160, 140, 210),
       (80, 160, 210, 280),
       (80, 160, 280, 360), (80, 160, 360, 430), (80, 160, 430, 500), (80, 160, 500, 570), (80, 160, 570, 640),
       (160, 240, 0, 70),
       (160, 240, 70, 140), (160, 240, 140, 210), (160, 240, 210, 280), (160, 240, 280, 360), (160, 240, 360, 430),
       (160, 240, 430, 500),
       (160, 240, 500, 570), (160, 240, 570, 640), (240, 320, 0, 70), (240, 320, 70, 140), (240, 320, 140, 210),
       (240, 320, 210, 280),
       (240, 320, 280, 360), (240, 320, 360, 430), (240, 320, 430, 500), (240, 320, 500, 570), (240, 320, 570, 640),
       (320, 400, 0, 70), (320, 400, 70, 140), (320, 400, 140, 210), (320, 400, 210, 280), (320, 400, 280, 360),
       (320, 400, 360, 430),
       (320, 400, 430, 500), (320, 400, 500, 570), (320, 400, 570, 640), (400, 480, 0, 70), (400, 480, 70, 140),
       (400, 480, 140, 210),
       (400, 480, 210, 280), (400, 480, 280, 360), (400, 480, 360, 430), (400, 480, 430, 500), (400, 480, 500, 570),
       (400, 480, 570, 640)]


def obscle():
    global state, org_img, step, reset, skip
    if state == 3:
        print('进入obscle')
        count = 0
        step = -1
        k = 1
        isgo = False
        #SSR.running_action_group("00", 1)  # stand
        straight = False  # 直行信号
        left = False  # 左移信号
        left2 = False  # 遇到右边缘且前方有障碍
        right = False  # 右移
        right2 = False  # 遇到左边缘且前方有障碍
    else:
        return
    while state == 3:
        if reset == 1:  # 是否为重置情况
            PWMServo.setServo(1, 2090, 500)
            PWMServo.setServo(2, 1480, 500)
            straight = False  # 直行信号
            left = False  # 左移信号
            left2 = False  # 遇到右边缘且前方有障碍
            right = False  # 右移
            right2 = False  # 遇到左边缘且前方有障碍
            reset = 0
            step = 0
            cv2.destroyAllWindows()
            SSR.running_action_group('0', 1)
        elif skip == 1:  # 跳至下一关
            cv2.destroyAllWindows()
            state += 1
            skip = 0
            state_sel = None
            SSR.running_action_group('666', 1)
            time.sleep(5)
            break
        if step == -1:  # 考虑复位置零，直接摆好位置的情况，-1这一步不用做
            direction(640, 480, 1)  # sel == 1:getLineSumContour  sel == 2:getLineSumContour
            PWMServo.setServo(1, 1500, 500)
            PWMServo.setServo(2, 1480, 500)
            time.sleep(0.5)
            PWMServo.setServo(1, 2090, 500)
            PWMServo.setServo(2, 1480, 500)
            time.sleep(0.2)
            step = 0
        elif step == 0 or step == 1:
            OrgFrame = org_img
            cv2.imshow('init', OrgFrame)
            hsv = cv2.cvtColor(OrgFrame, cv2.COLOR_BGR2HSV)
            hsv = cv2.GaussianBlur(hsv, (3, 3), 0)
            Imask = cv2.inRange(hsv, color_dist['black_obscle']['Lower'], color_dist['black_obscle']['Upper'])
            #Imask = cv2.erode(Imask, None, iterations=2)
            Imask = cv2.dilate(Imask, np.ones((3, 3), np.uint8), iterations=2)
            cv2.imshow('black', Imask)
            Imask2 = cv2.inRange(hsv, color_dist['blue']['Lower'], color_dist['blue']['Upper'])
            Imask2 = cv2.erode(Imask2, None, iterations=2)
            Imask2 = cv2.dilate(Imask2, np.ones((3, 3), np.uint8), iterations=2)
            _, cnts2, hierarchy2 = cv2.findContours(Imask2, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
            cv2.imshow('blue', Imask2)
            obscle_area_blue = 0
            # 当遇到蓝色门槛时停止
            for c in cnts2:
                obscle_area_blue += math.fabs(cv2.contourArea(c))
            if obscle_area_blue > 0.15 * 640 * 480:
                cv2.destroyAllWindows()
                SSR.running_action_group('157',1)
                state = 4
                break
            # 划分roi区域，对每个区域检测边缘，得到最大边缘，如果存在则该区域为障碍
            n = -1
            obscle_list = []
            obscle_list_last = []
            for r in roi:
                n += 1
                blobs = Imask[r[0]:r[1], r[2]:r[3]]
                _, cnts, hierarchy = cv2.findContours(blobs, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
                cnt_large = getAreaMaxContour2(cnts, 10)
                if cnt_large is not None:
                    # print(area_max)
                    lefttop_x = r[2]
                    lefttop_y = r[0]
                    rightbottom_x = r[3]
                    rightbottom_y = r[1]
                    cv2.rectangle(OrgFrame, (lefttop_x, lefttop_y), (rightbottom_x, rightbottom_y), (0, 255, 255), 2)
                    obscle_list.append([n // 9, n % 9])
            s1 = [[5, 4], [5, 3], [5, 2], [5, 1], [5, 5], [5, 6], [5, 7], [5, 0], [5, 8]]
            s1_1 = [[5, 4], [5, 3], [5, 2], [5, 1], [5, 5], [5, 6], [5, 7]]
            s1_2 = [[5, 4], [5, 3], [5, 2], [5, 1], [5, 5], [5, 6], [5, 7], [5, 8]]
            s1_3 = [[5, 4], [5, 3], [5, 2], [5, 1], [5, 5], [5, 6], [5, 7], [5, 0]]
            s2_1 = [[5, 1], [5, 0], [5, 3], [5, 2]]
            s2_2 = [[5, 4], [5, 5], [5, 6]]
            s3_1 = [[5, 5], [5, 6], [5, 7], [5, 8]]
            s3_2 = [[5, 4], [5, 3], [5, 2]]
            s4_1 = [[5, 1], [5, 0], [5, 3], [5, 2], [5, 4], [5, 5], [5, 6]]
            s4_2 = [[5, 7], [5, 8]]
            s5_1 = [[5, 5], [5, 6], [5, 7], [5, 8], [5, 3], [5, 2], [5, 4]]
            s5_2 = [[5, 1], [5, 0]]
            s_edge_left = [[4, 0], [3, 0], [2, 0], [1, 0]]
            s_edge_right = [[4, 8], [3, 8], [2, 8], [1, 8]]
            s_adjust = [[4, 4], [4, 3], [4, 2], [4, 1], [4, 5], [4, 6], [4, 7], [4, 0], [4, 8]]
            s_under=[[5, 4], [5, 3], [5, 2], [5, 1], [5, 5], [5, 6], [5, 7], [5, 0], [5, 8],[4, 4], [4, 3], [4, 2], [4, 1], [4, 5], [4, 6], [4, 7], [4, 0], [3, 8],[3, 4], [3, 3], [3, 2], [3, 1], [3, 5], [3, 6], [3, 7], [3, 0], [3, 8]]
            # if all(s not in obscle_list for s in s1_1) and (all(s not in obscle_list for s in s1_2) or ([5,0] in obscle_list and all(s in obscle_list for s in s_edge_left)) or ([5,8] in obscle_list and all(s in obscle_list for s in s_edge_right))):
            if all(s not in obscle_list for s in s1):
                straight = True
                left = False
                left2 = False
                right = False
                right2 = False
            elif all(s in obscle_list for s in s_edge_left) and any(s in obscle_list for s in s1_2):
                straight = False
                left = False
                left2 = False
                right = False
                right2 = True
                if [5, 4] in obscle_list:
                    k = 2
                elif [5, 5] in obscle_list or [5, 6] in obscle_list:
                    k = 4
                elif [5, 7] in obscle_list or [5, 8] in obscle_list:
                    k = 6
            elif all(s in obscle_list for s in s_edge_right) and any(s in obscle_list for s in s1_3):
                straight = False
                left = False
                left2 = True
                right = False
                right2 = False
                if [5, 4] in obscle_list:
                    k = 2
                elif [5, 3] in obscle_list or [5, 2] in obscle_list:
                    k = 4
                elif [5, 1] in obscle_list or [5, 0] in obscle_list:
                    k = 6
            elif all(s not in obscle_list for s in s2_1) and all(s not in obscle_list_last for s in s2_1) and any(
                    s in obscle_list for s in s2_2):
                straight = False
                left = True
                left2 = False
                right = False
                right2 = False
            elif all(s not in obscle_list for s in s3_1) and all(s not in obscle_list_last for s in s3_1) and any(
                    s in obscle_list for s in s3_2):
                straight = False
                left = False
                left2 = False
                right = True
                right2 = False
            elif all(s not in obscle_list for s in s4_1) and all(s not in obscle_list_last for s in s2_1) and any(
                    s in obscle_list for s in s4_2):
                straight = False
                left = True
                left2 = False
                right = False
                right2 = False
            elif all(s not in obscle_list for s in s5_1) and all(s not in obscle_list_last for s in s3_1) and any(
                    s in obscle_list for s in s5_2):
                straight = False
                left = False
                left2 = False
                right = True
                right2 = False
            # obscle_list_last = obscle_list
            cv2.putText(OrgFrame,
                        "straight:" + str(straight) + " left:" + str(left) + " right:" + str(right) + " left2:" + str(
                            left2) + " right2:" + str(right2),
                        (10, OrgFrame.shape[0] - 35), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            cv2.putText(OrgFrame, "step:" + str(step),
                        (10, OrgFrame.shape[0] - 55), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            # cv2.putText(OrgFrame,"finish:"+str(obscle_finish),(10,OrgFrame.shape[0]-75),cv2.FONT_HERSHEY_SIMPLEX,0.65,(0,0,255),2)
            cv2.imshow('frame', OrgFrame)
            cv2.waitKey(1)

            if step == 0:
                if all(s not in obscle_list for s in s_under): 
                    SSR.running_action_group("239", 1)  # 前进
                    count += 1
                    isgo = True
                else:
                    if isgo:
                        SSR.running_action_group('157', 1)

                    isgo = False
                    step = 1
                    time.sleep(0.2)
            elif step == 1:
                if all(s not in obscle_list for s in s_under):
                    step = 0
                elif (count >= 20) and any(s in obscle_list for s in s_adjust):
                    direction_only_angle(640, 480)
                    PWMServo.setServo(1, 2090, 500)
                    PWMServo.setServo(2, 1480, 500)
                    time.sleep(0.8)
                    count = 0
                elif straight:
                    SSR.running_action_group("232", 1)  # 前进
                    count += 1
                elif left:
                    SSR.running_action_group("160", 1)  # 左移
                elif right:
                    SSR.running_action_group("161", 1)  # 右移
                elif left2 is True:
                    SSR.running_action_group("160", k)  # 左移5次
                elif right2 is True:
                    SSR.running_action_group("161", k)  # 右移5次
                else:
                    SSR.running_action_group("00", 1)  # 静止


#################################################第四关：挡板###########################################
def baffle(resize_width, resize_height):
    global state, org_img, step, ori_width, ori_height, reset, skip
    if state == 4:
        PWMServo.setServo(1, 2100, 500)
        PWMServo.setServo(2, 1510, 500)
        print('进入baffle')
        time.sleep(0.6)
        step = 0
        baffle_dis_flag = False
        baffle_angle = 0
        notok = True
        see = False
        finish = False
        angle = 45
        dis = 0
        dis_flag = False
        angle_flag = False
    else:
        return
    while state == 4:
        if reset == 1:  # 是否为重置情况
            PWMServo.setServo(1, 2100, 500)
            PWMServo.setServo(2, 1510, 500)
            step = 0
            baffle_dis_flag = False
            baffle_angle = 0
            notok = True
            see = False
            finish = False
            angle = 45
            dis = 0
            dis_flag = False
            angle_flag = False
            reset = 0
            step = 0
            cv2.destroyAllWindows()
            SSR.running_action_group('0', 1)
        elif skip == 1:  # 跳至下一关
            cv2.destroyAllWindows()
            state += 1
            skip = 0
            state_sel = None
            SSR.running_action_group('666', 1)
            time.sleep(5)
            break
        if step == 0 or step == 1 or step==2:
            OrgFrame = org_img
            frame = cv2.resize(OrgFrame, (resize_width, resize_height), interpolation=cv2.INTER_LINEAR)
            cv2.imshow('init', frame)
            center = []
            # 开始处理图像
            hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
            hsv = cv2.GaussianBlur(hsv, (3, 3), 0)
            Imask = cv2.inRange(hsv, color_dist['blue']['Lower'], color_dist['blue']['Upper'])
            Imask = cv2.erode(Imask, None, iterations=2)
            Imask = cv2.dilate(Imask, np.ones((3, 3), np.uint8), iterations=2)
            cv2.imshow('color', Imask)
            _, cnts, hieracy = cv2.findContours(Imask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_L1)  # 找出所有轮廓
            cnt_large = getAreaMaxContour2(cnts, area=1000)
            y_max = 0
            if cnt_large is not None:
                rect = cv2.minAreaRect(cnt_large)  # 最小外接矩形
                box = np.int0(cv2.boxPoints(rect))  # 最小外接矩形的四个顶点
                box[0, 0] = int(leMap(box[0, 0], 0, resize_width, 0, ori_width))
                box[0, 1] = int(leMap(box[0, 1], 0, resize_height, 0, ori_height))
                box[1, 0] = int(leMap(box[1, 0], 0, resize_width, 0, ori_width))
                box[1, 1] = int(leMap(box[1, 1], 0, resize_height, 0, ori_height))
                box[2, 0] = int(leMap(box[2, 0], 0, resize_width, 0, ori_width))
                box[2, 1] = int(leMap(box[2, 1], 0, resize_height, 0, ori_height))
                box[3, 0] = int(leMap(box[3, 0], 0, resize_width, 0, ori_width))
                box[3, 1] = int(leMap(box[3, 1], 0, resize_height, 0, ori_height))
                pt1_x, pt1_y = box[0, 0], box[0, 1]
                pt3_x, pt3_y = box[2, 0], box[2, 1]
                center_x = int((pt1_x + pt3_x) / 2)
                center_y = int((pt1_y + pt3_y) / 2)
                center.append([center_x, center_y])
                cv2.drawContours(OrgFrame, [box], -1, [0, 0, 255, 255], 3)
                cv2.circle(OrgFrame, (center_x, center_y), 10, (0, 0, 255), -1)  # 画出中心点
                # 求得大矩形的旋转角度，if条件是为了判断长的一条边的旋转角度，因为box存储的点的顺序不确定\
                if math.sqrt(math.pow(box[3, 1] - box[0, 1], 2) + math.pow(box[3, 0] - box[0, 0], 2)) > math.sqrt(
                        math.pow(box[3, 1] - box[2, 1], 2) + math.pow(box[3, 0] - box[2, 0], 2)):
                    baffle_angle = - math.atan((box[3, 1] - box[0, 1]) / (box[3, 0] - box[0, 0])) * 180.0 / math.pi
                else:
                    baffle_angle = - math.atan(
                        (box[3, 1] - box[2, 1]) / (box[3, 0] - box[2, 0])) * 180.0 / math.pi  # 负号是因为坐标原点的问题
                if center_y > y_max:
                    y_max = center_y
            baffle_dis = y_max
            if baffle_dis > 240:
                baffle_dis_flag = True
            cv2.putText(OrgFrame, "dis:" + str(baffle_dis),
                        (10, OrgFrame.shape[0] - 35), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
        
            cv2.putText(OrgFrame, "dis_flag:" + str(baffle_dis_flag),
                        (10, OrgFrame.shape[0] - 55), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
        
            cv2.putText(OrgFrame, "angle:" + str(baffle_angle),
                        (10, OrgFrame.shape[0] - 75), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            cv2.imshow('OrgFrame', OrgFrame)
            cv2.waitKey(1)
        
            if step == 0:
                SSR.running_action_group('239',1)
                SSR.running_action_group('157',1)
                step=1
            elif step==1:
                if baffle_angle > 3:
                    SSR.running_action_group("82", 1)
                elif baffle_angle < -3:
                    SSR.running_action_group("84", 1)
                else:
                    step=2
                time.sleep(0.2)
                
            elif step == 2:
                SSR.running_action_group('239', 1)  # 前进1步
                if baffle_dis_flag:
                    step = 3
                    SSR.running_action_group('157',1)
                    PWMServo.setServo(1, 1800, 500)
                    PWMServo.setServo(2, 1380, 500)
                    time.sleep(0.6)
                    cv2.destroyAllWindows()
        elif step == 3:
            #print('2')
            
            OrgFrame = org_img
            frame = cv2.resize(OrgFrame, (resize_width, resize_height), interpolation=cv2.INTER_LINEAR)
            cv2.imshow('init', frame)
            center = []
            # 开始处理图像
            hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
            hsv = cv2.GaussianBlur(hsv, (3, 3), 0)
            Imask = cv2.inRange(hsv, color_dist['black']['Lower'], color_dist['black']['Upper'])
            Imask = cv2.erode(Imask, None, iterations=2)
            Imask = cv2.dilate(Imask, np.ones((3, 3), np.uint8), iterations=2)
            cv2.imshow('black', Imask)
            cv2.waitKey(1)
            _, cnts, hierarchy = cv2.findContours(Imask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_TC89_L1)  # 找出所有轮廓
            cnt_large = getAreaMaxContour2(cnts, area=100)  # 找到最大面积的轮廓
            
            if cnt_large is not None:
                SSR.running_action_group('160',1)
            else:
                SSR.running_action_group('00',1)
                PWMServo.setServo(1, 2200, 500)
                PWMServo.setServo(2, 1510, 500)
                SSR.running_action_group("237", 3)  # 前进3步
                SSR.running_action_group("242", 1)
                #time.sleep(0.5)
                SSR.running_action_group("00", 1)
                SSR.running_action_group("36", 1)  # 左转
                time.sleep(0.3)
                SSR.running_action_group("36", 1)  # 左转
                time.sleep(0.3)
                SSR.running_action_group("36", 1)  # 左转
                time.sleep(0.3)
                SSR.running_action_group("36", 1)  # 左转
                time.sleep(0.3)
                SSR.running_action_group("82", 2)  # 左转
                cv2.destroyAllWindows()
                step = 0
                state = 5
                break
        

#################################################第五关：过门###########################################
def door(resize_width, resize_height):
    global state, state_sel, org_img, state_sel, reset, skip
    if state == 5:
        print('进入door')
        PWMServo.setServo(1, 2000, 500)
        PWMServo.setServo(2, 1510, 500)
        notok = True
        #area_green = 0
        stop = False
        ncount = 0
    else:
        return
    while state == 5:
        if reset == 1:  # 是否为重置情况
            PWMServo.setServo(1, 2000, 500)
            PWMServo.setServo(2, 1510, 500)
            notok = True
            #area_green = 0
            stop = False
            ncount = 0
            reset = 0
            cv2.destroyAllWindows()
            SSR.running_action_group('0', 1)
        elif skip == 1:  # 跳至下一关
            cv2.destroyAllWindows()
            state += 1
            skip = 0
            state_sel = None
            SSR.running_action_group('666', 1)
            time.sleep(5)
            break
        state_sel = state_choose(320, 240)
        if state_sel is not None:
            print('state_sel: %s' % state_sel)
            SSR.running_action_group('157', 1)
            state = 6
            cv2.destroyAllWindows()
            break

        if notok:
            #print('1')
            direction(640,480,1)
            #print('2')
            PWMServo.setServo(1, 2000, 500)
            PWMServo.setServo(2, 1510, 500)
            time.sleep(0.6)
            notok=False
        else:
            SSR.running_action_group('239', 1)  # 前进
            ncount += 1
            if ncount == 6:
                SSR.running_action_group("157", 1) 
                notok = True
                ncount = 0


################################################第六关：独木桥##########################################
def bridge(r_w, r_h):
    global state, state_sel, org_img, step, reset, skip, debug
    if (state == 2 or state == 6 or state == 8) and state_sel == 'bridge':  # 初始化
        print('进入bridge')
        step = 0
        cnt = 0
        PWMServo.setServo(1, 2000, 500)  # NO3机器人，初始云台角度
        PWMServo.setServo(2, 1510, 500)
        time.sleep(0.5)
        #SSR.running_action_group('00', 1)
        # time.sleep(0.5) #等待摄像头稳定
    elif (state == 2 or state == 6 or state == 8) and state_sel is None:  # 跳关情况
        state_sel = state_choose(320, 240)
        return
    else:
        return
    while (state == 2 or state == 6 or state == 8) and state_sel == 'bridge':  # 初始化
        if reset == 1:  # 是否为重置情况
            # PWMServo.setServo(1, 2100, 500) # NO2机器人，初始云台角度
            # PWMServo.setServo(2, 1510, 500)
            PWMServo.setServo(1, 2000, 500)  # NO3机器人，初始云台角度
            PWMServo.setServo(2, 1510, 500)
            reset = 0
            step = 0
            cnt = 0
            cv2.destroyAllWindows()
            SSR.running_action_group('0', 1)
        if skip == 1:  # 跳至下一关
            cv2.destroyAllWindows()
            state += 1
            skip = 0
            state_sel = None
            SSR.running_action_group('666', 1)
            time.sleep(5)
            break
        t1 = cv2.getTickCount()
        border = cv2.copyMakeBorder(org_img, 12, 12, 16, 16, borderType=cv2.BORDER_CONSTANT,
                                    value=(255, 255, 255))  # 扩展白边，防止边界无法识别
        org_img_copy = cv2.resize(border, (r_w, r_h), interpolation=cv2.INTER_CUBIC)  # 将图片缩放
        frame_gauss = cv2.GaussianBlur(org_img_copy, (3, 3), 0)  # 高斯模糊
        frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        frame_green = cv2.inRange(frame_hsv, color_range['green_bridge'][0],
                                  color_range['green_bridge'][1])  # 对原图像和掩模(颜色的字典)进行位运算
        opened = cv2.morphologyEx(frame_green, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))  # 开运算 去噪点
        closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8))  # 闭运算 封闭连接
        # print(closed)

        (image, contours, hierarchy) = cv2.findContours(closed, cv2.RETR_LIST,
                                                        cv2.CHAIN_APPROX_NONE)  # 找出轮廓cv2.CHAIN_APPROX_NONE

        areaMaxContour, area_max = getAreaMaxContour(contours)  # 找出最大轮廓
        percent = round(area_max * 100 / (r_w * r_h), 2)
        if debug:
            cv2.imshow('closed', closed)  # 显示图像
            cv2.drawContours(org_img_copy, contours, -1, (255, 0, 255), 1)
            cv2.putText(org_img_copy, 'area:' + str(percent) + '%', (30, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                        2)  # (0, 0, 255)BGR
        if areaMaxContour is not None:
            rect = cv2.minAreaRect(areaMaxContour)
            # center, w_h, angle = rect  # 中心点 宽高 旋转角度
            box = np.int0(cv2.boxPoints(rect))  # 点的坐标


            top_left = areaMaxContour[0][0]
            top_right = areaMaxContour[0][0]
            bottom_left = areaMaxContour[0][0]
            bottom_right = areaMaxContour[0][0]
            for c in areaMaxContour:  # 遍历找到四个顶点
                if c[0][0] + 1.5 * c[0][1] < top_left[0] + 1.5 * top_left[1]:
                    top_left = c[0]
                if (r_w - c[0][0]) + 1.5 * c[0][1] < (r_w - top_right[0]) + 1.5 * top_right[1]:
                    top_right = c[0]
                if c[0][0] + 1.5 * (r_h - c[0][1]) < bottom_left[0] + 1.5 * (r_h - bottom_left[1]):
                    bottom_left = c[0]
                if c[0][0] + 1.5 * c[0][1] > bottom_right[0] + 1.5 * bottom_right[1]:
                    bottom_right = c[0]
            # angle_top = - math.atan((top_right[1] - top_left[1]) / (top_right[0] - top_left[0])) * 180.0 / math.pi
            # angle_bottom = - math.atan((bottom _right[1] - bottom_left[1]) / (bottom _right[0] - bottom_left[0])) * 180.0 / math.pi
            top_center_x = int((top_right[0] + top_left[0]) / 2)
            top_center_y = int((top_right[1] + top_left[1]) / 2)
            bottom_center_x = int((bottom_right[0] + bottom_left[0]) / 2)
            bottom_center_y = int((bottom_right[1] + bottom_left[1]) / 2)
            center_x = int((top_center_x + bottom_center_x) / 2)
            if debug:
                cv2.drawContours(org_img_copy, [box], 0, (0, 0, 255), 2)  # 将大矩形画在图上
                cv2.circle(org_img_copy, (top_right[0], top_right[1]), 5, [0, 255, 255], 2)
                cv2.circle(org_img_copy, (top_left[0], top_left[1]), 5, [0, 255, 255], 2)
                cv2.circle(org_img_copy, (bottom_right[0], bottom_right[1]), 5, [0, 255, 255], 2)
                cv2.circle(org_img_copy, (bottom_left[0], bottom_left[1]), 5, [0, 255, 255], 2)
                cv2.circle(org_img_copy, (top_center_x, top_center_y), 5, [0, 255, 255], 2)
                cv2.circle(org_img_copy, (bottom_center_x, bottom_center_y), 5, [0, 255, 255], 2)
                cv2.line(org_img_copy, (top_center_x, top_center_y), (bottom_center_x, bottom_center_y), [0, 255, 255],
                         2)  # 画出上下中点连线
            if math.fabs(top_center_x - bottom_center_x) <= 1:  # 得到连线的角度
                angle = 90
            else:
                angle = - math.atan((top_center_y - bottom_center_y) / (top_center_x - bottom_center_x)) * 180.0 / math.pi
        else:
            angle = 90
            center_x = 0.5*r_w
        if debug:
            t2 = cv2.getTickCount()
            time_r = (t2 - t1) / cv2.getTickFrequency()
            fps = 1.0 / time_r
            # print('fps: %d' %(fps))
            # cv2.putText(org_img_copy, "step:" + str(step), (30, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0), 2)  # (0, 0, 255)BGR
            cv2.putText(org_img_copy, "fps:" + str(int(fps)), (30, 300), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                        2)  # (0, 0, 255)BGR
            cv2.putText(org_img_copy, "angle:" + str(int(angle)), (30, 400), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                        2)  # (0, 0, 255)BGR
            cv2.putText(org_img_copy, "center_x:" + str(int(center_x)), (30, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.65,
                        (0, 0, 0), 2)  # (0, 0, 255)BGR
            # cv2.moveWindow('orgFrame', img_center_x, 100)  # 显示框位置
            cv2.imshow('org_img_copy', org_img_copy)  # 显示图像
            cv2.waitKey(1)

        if step == 0:  # 接近独木桥阶段
            SSR.running_action_group('239', 1)
            if percent > 30:  # 绿色区域大于30%
                #print('percent > 30')
                SSR.running_action_group('157', 1)  # 立正
                step = 1
        elif step == 1:# 独木桥阶段
            if percent < 5:  # 接近终点，直行2步离开
                SSR.running_action_group('239', 2)
                SSR.running_action_group('157', 1)
                state_sel = None
                state += 1
                step = -1 #出于踢球双线程的考虑
                cv2.destroyAllWindows()
                break
            if 0 < angle < 85:  # 右转
                SSR.running_action_group('203', 1)
            elif -85 < angle < 0:  # 左转
                SSR.running_action_group('202', 1)
            elif angle <= -85 or angle >= 85:
                if center_x > 0.54 * r_w:  # 右移
                    SSR.running_action_group('161', 1)
                elif center_x < 0.48 * r_w:  # 左移
                    SSR.running_action_group('160', 1)
                    # time.sleep(0.1)
                elif 0.46 * r_w <= center_x <= 0.54 * r_w:#走三步
                    SSR.running_action_group('239', 4)
                    SSR.running_action_group('157', 1)
                    cnt += 1
                    if cnt == 3:  # 走四次后更低头
                        PWMServo.setServo(1, 2200, 500)
                        PWMServo.setServo(2, 1510, 500)
                        time.sleep(0.6)


################################################第七关：踢球进洞########################################
golf_angle_ball = 90
golf_angle_hole = 45
golf_angle = 0
golf_dis = 0
golf_dis_y = 0
golf_dis_flag = False
golf_angle_flag = False
golf_dis_start = True
golf_angle_start = False
golf_ok = False
golf_ball = False
golf_hole = False
def look():
    global golf_dis_y
    PWMServo.setServo(1, 2180, 500)
    PWMServo.setServo(2, 1510, 500)
    time.sleep(1)
    print(golf_dis_y)
    if (golf_dis_y > 260):
        # print('1')
        SSR.running_action_group("181", 1)  # move back
        golf_dis_y = 230
    elif (golf_dis_y < 190):
        SSR.running_action_group("232", 1)  # move back
        golf_dis_y = 230
    PWMServo.setServo(1, 1800, 500)
    PWMServo.setServo(2, 1510, 500)
    time.sleep(0.6)

def move():
    global step, state, reset, skip
    global golf_angle_hole
    global golf_angle_ball, golf_angle
    global golf_dis, golf_dis_y
    global golf_angle_flag, golf_dis_flag
    global golf_angle_start, golf_dis_start
    global golf_ok
    global golf_hole, golf_ball
    while state == 7:
        if reset == 1 or skip == 1:
            return
        if step == 0:
            SSR.running_action_group("239", 1)
            #time.sleep(0.1)
            if golf_ball:
                SSR.running_action_group("239", 2)
                #time.sleep(0.1)
                SSR.running_action_group('157', 1)
                step = 1
        elif step == 1:
            if golf_dis < 240:
                SSR.running_action_group('160', 1)  # left move
            elif golf_dis > 310:
                SSR.running_action_group('161', 1)  # right move
            else:
                # SSR.running_action_group('0',1)
                step = 2
        elif step == 2:
            SSR.running_action_group("239", 1)
            #time.sleep(0.1)
            if golf_dis_y > 240:
                step = 3
        elif step == 3:
            if golf_dis < 240:
                SSR.running_action_group('160', 1)  # left move
            elif golf_dis > 310:
                SSR.running_action_group('161', 1)  # right move
            else:
                # SSR.running_action_group('0',1)
                PWMServo.setServo(1, 2180, 500)
                PWMServo.setServo(2, 1510, 500)
                time.sleep(0.6)
                step = 4
        elif step == 4:
            SSR.running_action_group("239", 1)
            #time.sleep(0.1)
            if golf_ball:
                SSR.running_action_group('157', 1)
                SSR.running_action_group("232", 1)
                step = 5
        elif step == 5:
            if golf_ok:
                # print(dis_y)
                time.sleep(0.5)
                print(golf_dis_y)
                if (golf_dis_y < 265):
                    SSR.running_action_group("232", 1)
                else:
                    SSR.running_action_group("137", 1)
                    step = 6  # kick the ball

                # SSR.running_action_group("00",1)
                time.sleep(0.5)
            elif golf_dis_start:
                if (golf_dis_y > 260):
                    # print('1')
                    SSR.running_action_group("181", 1)  # move back
                    golf_dis_y = 230
                if (160 < golf_dis < 195) and golf_angle_flag:
                    golf_ok = True
                elif (golf_dis < 160):
                    SSR.running_action_group("240", 1)
                    golf_angle_flag = False
                elif (golf_dis > 195):
                    SSR.running_action_group("241", 1)
                    golf_angle_flag = False
                else:
                    golf_dis_flag = True
                    golf_angle = golf_angle_ball
                    PWMServo.setServo(1, 1800, 500)
                    PWMServo.setServo(2, 1510, 500)
                    time.sleep(0.6)
                    golf_angle_start = True
                    golf_dis_start = False
                SSR.running_action_group("00", 1)
            elif golf_angle_start:
                if not golf_hole:
                    look()
                    SSR.running_action_group("4", 1)
                else:
                    golf_flag = ((golf_angle_hole > 0) and (golf_angle > 0)) or ((golf_angle_hole < 0) and (golf_angle < 0))
                    if (golf_angle_hole - golf_angle < -6 and golf_flag) or (
                            (golf_angle_hole > 0) and (golf_angle < 0) and (golf_angle - golf_angle_hole + 180 > 6)):
                        if (golf_angle_hole - golf_angle < -30 and golf_flag) or (
                            (golf_angle_hole > 0) and (golf_angle < 0) and (golf_angle - golf_angle_hole + 180 > 30)):
                            look()
                            SSR.running_action_group("4", 1)
                        else:
                            look()
                            SSR.running_action_group("203", 1)
                    elif (golf_angle_hole - golf_angle > 6 and golf_flag) or (
                            (golf_angle_hole < 0) and (golf_angle > 0) and (golf_angle_hole - golf_angle + 180 > 6)):
                        if (golf_angle_hole - golf_angle > 30 and golf_flag) or (
                            (golf_angle_hole < 0) and (golf_angle > 0) and (golf_angle_hole - golf_angle + 180 > 30)):
                            look()
                            SSR.running_action_group("3", 1)
                        else:
                            look()
                            SSR.running_action_group("202", 1)
                    else:
                        golf_angle_flag = True
                        golf_angle_ball = golf_angle
                        PWMServo.setServo(1, 2180, 500)
                        PWMServo.setServo(2, 1510, 500)
                        time.sleep(0.6)
                        golf_angle_start = False
                        golf_dis_start = True
                SSR.running_action_group("00", 1)
                # SSR.running_action_group("0",1)
            else:
                SSR.running_action_group("00", 1)
        elif step == 6:
            SSR.running_action_group("00", 1)
            SSR.running_action_group('239',3)
            SSR.running_action_group("157",1)
            time.sleep(0.3)
            SSR.running_action_group("36",1)
            time.sleep(0.3)
            SSR.running_action_group("36",1)
            time.sleep(0.3)
            SSR.running_action_group("36",1)
            time.sleep(0.3)
            SSR.running_action_group("36",1)
            time.sleep(0.3)
            SSR.running_action_group("36",1)
            time.sleep(0.3)
            SSR.running_action_group("36",1)
            time.sleep(0.3)
            SSR.running_action_group("202",2)
            time.sleep(0.3)
            
            PWMServo.setServo(1, 1800, 500)
            PWMServo.setServo(2, 850, 500)
            time.sleep(0.6)
            step = 7
            cv2.destroyAllWindows()
            return

th2 = threading.Thread(target=move)
th2.setDaemon(True)  # 设置为后台线程，这里默认是False，设置为True之后则主线程不用等待子线程

def kick_ball(resize_width, resize_height):
    global state, state_sel, step, reset, skip
    global golf_angle_hole
    global golf_angle_ball, golf_angle
    global golf_dis, golf_dis_y
    global golf_ball, golf_hole
    if state == 7:
        print('进入kick_ball')
        step = -1
        #step = 0
        sum_contours = np.array([[[0, 0]], [[0, 1]], [[1, 1]], [[1, 0]]])
        time.sleep(0.6)
        notok = True
        stop = False
        ncount = 0
    else:
        return
    while state == 7:
        if reset == 1:  # 是否为重置情况
            step = 0
            PWMServo.setServo(1, 1800, 500)
            PWMServo.setServo(2, 1510, 500)
            cv2.destroyAllWindows()
            SSR.running_action_group('0', 1)
            reset = 0
            #th2.start()
        if skip == 1:  # 跳至下一关
            cv2.destroyAllWindows()
            state += 1
            skip = 0
            state_sel = None
            SSR.running_action_group('666', 1)
            time.sleep(5)
            break
        if step == -1:
            direction(640,480,2)
            PWMServo.setServo(1, 1800, 500)
            PWMServo.setServo(2, 1510, 500)
            time.sleep(0.6)
            step = 0
            
            th2.start()
        if 0<=step <= 6: #踢球的六步
            OrgFrame = org_img
            cv2.imshow("init", OrgFrame)
            img_h, img_w = OrgFrame.shape[:2]
            bottom_center = (int(200), int(img_h))
            bottom_center2 = (int(190), int(0))
            # 开始处理图像
            hsv = cv2.cvtColor(OrgFrame, cv2.COLOR_BGR2HSV)
            hsv = cv2.GaussianBlur(hsv, (3, 3), 0)
            Imask = cv2.inRange(hsv, color_dist['blue']['Lower'], color_dist['blue']['Upper'])
            Imask = cv2.erode(Imask, None, iterations=2)
            Imask = cv2.dilate(Imask, np.ones((3, 3), np.uint8), iterations=2)

            Imask2 = cv2.inRange(hsv, color_dist['red']['Lower'], color_dist['red']['Upper'])
            Imask2 = cv2.erode(Imask2, None, iterations=2)
            Imask2 = cv2.dilate(Imask2, np.ones((3, 3), np.uint8), iterations=2)
            cv2.imshow('Imask', Imask)
            cv2.imshow('Imask2', Imask2)
            _, cnts, hierachy = cv2.findContours(Imask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            for i in range(0, len(cnts)):  # 初始化sum，使其等于其中一个c，便于之后拼接的格式统一
                area = cv2.contourArea(cnts[i])
                # if ori_width * ori_height * 0.001 < area < ori_width * ori_height * 0.9:
                if 640 * 480 * 0.001 < area < 640 * 480 * 0.45:  # 去掉很小的干扰轮廓以及最大的图像边界
                    sum_contours = cnts[i]
                    break
                else:
                    continue
            for c in cnts:
                area = cv2.contourArea(c)
                if 640 * 480 * 0.001 < area < 640 * 480 * 0.45:
                    sum_contours = np.concatenate((sum_contours, c), axis=0)
                else:
                    continue
            sum_area = cv2.contourArea(sum_contours)
            if sum_area > 10:
                cnt_large = sum_contours
            else:
                cnt_large = None
            _, cnts2, hierachy2 = cv2.findContours(Imask2, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            cnt_large2 = getAreaMaxContour2(cnts2, 100)
            center = [0, 0]
            if cnt_large is not None:
                golf_hole = True
                (circle_x, circle_y), radius = cv2.minEnclosingCircle(cnt_large)
                center = (int(circle_x)+80, int(circle_y))
                radius = int(radius)
                cv2.circle(OrgFrame, center, radius, (255, 0, 0), 2)
                # ellipse = cv2.fitEllipse(cnt_large)
                # cv2.ellipse(OrgFrame,ellipse,(255,255,0),2)
                cv2.line(OrgFrame, center, bottom_center, (255, 0, 0), 2)
                if (center[0] - bottom_center[0]) == 0:
                    golf_angle_hole = 90
                else:
                    golf_angle_hole = - math.atan(
                        (center[1] - bottom_center[1]) / (center[0] - bottom_center[0])) * 180.0 / math.pi
            else:
                golf_hole = False
            if cnt_large2 is not None:
                golf_ball = True
                (circle_x2, circle_y2), radius2 = cv2.minEnclosingCircle(cnt_large2)
                center2 = (int(circle_x2), int(circle_y2))
                radius2 = int(radius2)
                cv2.circle(OrgFrame, center2, radius2, (0, 255, 255), 2)
                cv2.line(OrgFrame, center2, bottom_center2, (0, 255, 255), 2)
                # ellipse = cv2.fitEllipse(cnt_large)
                # cv2.ellipse(OrgFrame,ellipse,(255,255,0),2)
                if (center2[0] - bottom_center2[0]) == 0:
                    golf_angle_ball = 90
                else:
                    golf_angle_ball = - math.atan(
                        (center2[1] - bottom_center2[1]) / (center2[0] - bottom_center2[0])) * 180.0 / math.pi
                golf_dis = int(circle_x2)
                golf_dis_y = int(circle_y2)
            else:
                golf_ball = False

            cv2.putText(OrgFrame, "step:" + str(step),
                        (10, OrgFrame.shape[0] - 35), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            cv2.putText(OrgFrame, "angle_hole:" + str(golf_angle_hole),
                        (10, OrgFrame.shape[0] - 55), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            cv2.putText(OrgFrame, "dis:" + str(golf_dis),
                        (10, OrgFrame.shape[0] - 75), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            cv2.putText(OrgFrame, "angle_ball:" + str(golf_angle_ball),
                        (10, OrgFrame.shape[0] - 95), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            cv2.putText(OrgFrame, "dis_y:" + str(golf_dis_y),
                        (10, OrgFrame.shape[0] - 115), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            cv2.putText(OrgFrame, "angle:" + str(golf_angle),
                        (10, OrgFrame.shape[0] - 135), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            cv2.imshow('Frame', OrgFrame)
            cv2.waitKey(1)

        elif step == 7: #直行至第八关的一步
            
            
            # cv2.putText(frame, "notok:" + str(notok),
            #             (10, frame.shape[0] - 75), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            # cv2.putText(frame, "stop:" + str(stop),
            #             (10, frame.shape[0] - 95), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            #
            # cv2.putText(frame, "area_green:" + str(area_green),
            #             (10, frame.shape[0] - 135), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)
            # cv2.imshow('frame', frame)
            # cv2.waitKey(1)

            if notok:
                #print('1')
                direction(640,480,1)
                #print('2')
                PWMServo.setServo(1, 2000, 500)
                PWMServo.setServo(2, 1510, 500)
                time.sleep(0.6)
                notok=False
            else:
                SSR.running_action_group('239', 1)  # 前进
                ncount += 1
                if ncount > 10:
                    state_sel = state_choose(320, 240)
                    if state_sel is not None:
                        print(state_sel)
                        SSR.running_action_group('157', 1)
                        #direction(640,480,1)
                        state = 8
                        cv2.destroyAllWindows()
                        break
                if ncount == 10:
                    SSR.running_action_group("157", 1) 
                    notok = True
                    #ncount = 0


##################################################第八关：过洞###############################################
def square_hole(r_w, r_h):
    global org_img, state, state_sel, step, reset, skip, debug
    if (state == 2 or state == 6 or state == 8) and state_sel == 'hole':  # 初始化
        print('进入square_hole')
        step = 0
        PWMServo.setServo(1, 2100, 500)  # NO3机器人，初始云台角度
        #PWMServo.setServo(1, 1810, 500)  # NO3机器人，初始云台角度
        PWMServo.setServo(2, 1500, 500)
        time.sleep(0.5)
        #SSR.running_action_group('00', 1)
        # time.sleep(0.5) #等待摄像头稳定
    elif (state == 2 or state == 6 or state == 8) and state_sel is None:  # 跳关情况
        state_sel = state_choose(320, 240)
        return
    else:
        return
    while (state == 2 or state == 6 or state == 8) and state_sel == 'hole':  # 初始化
        if reset == 1:  # 是否为重置情况
            # PWMServo.setServo(1, 2100, 500) # NO2机器人，初始云台角度
            # PWMServo.setServo(2, 1510, 500)
            PWMServo.setServo(1, 2100, 500)  # NO3机器人，初始云台角度
            PWMServo.setServo(2, 1510, 500)
            reset = 0
            step = 0
            cv2.destroyAllWindows()
            SSR.running_action_group('0', 1)
        if skip == 1:  # 跳至下一关
            cv2.destroyAllWindows()
            state += 1
            skip = 0
            state_sel = None
            SSR.running_action_group('666', 1)
            time.sleep(5)
            break
        if step == 0:
            t1 = cv2.getTickCount()
            org_img_copy = cv2.resize(org_img, (r_w, r_h), interpolation=cv2.INTER_CUBIC)  # 将图片缩放
            frame_gauss = cv2.GaussianBlur(org_img_copy, (3, 3), 0)  # 高斯模糊
            frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
            frame = cv2.inRange(frame_hsv, color_range['yellow_hole'][0],
                                color_range['yellow_hole'][1])  # 对原图像和掩模(颜色的字典)进行位运算
            opened = cv2.morphologyEx(frame, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))  # 开运算 去噪点
            closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((9, 9), np.uint8))  # 闭运算 封闭连接
            if debug:
                cv2.imshow('closed', closed)  # 显示图像
            (image, contours, hierarchy) = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)  # 找出轮廓
            area_sum = getAreaSumContour(contours)
            percent = round(100 * area_sum / (r_w * r_h), 2)
            
            areaMaxContour, area_max = getAreaMaxContour(contours)  # 找出最大轮廓
            if areaMaxContour is not None:
                bottom_left = areaMaxContour[0][0]
                bottom_right = areaMaxContour[0][0]
                for c in areaMaxContour:  # 遍历找到四个顶
                    if c[0][0] + 1 * (r_h - c[0][1]) < bottom_left[0] + 1.* (r_h - bottom_left[1]):
                        bottom_left = c[0]
                    if c[0][0] + 1 * c[0][1] > bottom_right[0] + 1 * bottom_right[1]:
                        bottom_right = c[0]
                angle_bottom = - math.atan((bottom_right[1] - bottom_left[1]) / (bottom_right[0] - bottom_left[0])) * 180.0 / math.pi
                bottom_center_x = int((bottom_right[0] + bottom_left[0]) / 2)
                bottom_center_y = int((bottom_right[1] + bottom_left[1]) / 2)
                if debug:
                    cv2.circle(org_img_copy, (bottom_right[0], bottom_right[1]), 5, [0, 255, 255], 2)
                    cv2.circle(org_img_copy, (bottom_left[0], bottom_left[1]), 5, [0, 255, 255], 2)
                    cv2.circle(org_img_copy, (bottom_center_x, bottom_center_y), 5, [0, 255, 255], 2)
            else:
                angle_bottom = 0
                bottom_center_x = 0.5 * r_w
                bottom_center_y = 0
            if debug:
                t2 = cv2.getTickCount()
                time_r = (t2 - t1) / cv2.getTickFrequency()
                fps = 1.0 / time_r
                cv2.putText(org_img_copy, "step:" + str(step), (30, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                            2)  # (0, 0, 255)BGR
                cv2.putText(org_img_copy, "fps:" + str(int(fps)), (30, 300), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                            2)  # (0, 0, 255)BGR
                cv2.putText(org_img_copy, 'area: ' + str(percent) + '%', (30, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
                            (0, 0, 0), 2)
                cv2.putText(org_img_copy, '(x,y): ' + str(bottom_center_x) + ',' + str(bottom_center_y), (230, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
                            (0, 0, 0), 2)
                cv2.putText(org_img_copy, 'angle: ' + str(angle_bottom) , (230, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
                            (0, 0, 0), 2)
                cv2.imshow('org_img_copy', org_img_copy)  # 显示图像
                cv2.waitKey(1)
            
            if percent < 20:
                SSR.running_action_group('239', 1)
            elif percent >= 20:
                if bottom_center_y < 0.5*r_h:
                    SSR.running_action_group('239', 1)
                elif bottom_center_y >= 0.5*r_h:
                    if angle_bottom < -3:
                        SSR.running_action_group('203', 1)
                        #SSR.running_action_group('84', 1)
                        #SSR.running_action_group('203', 1)
                        time.sleep(0.2)
                    elif angle_bottom > 3:
                        SSR.running_action_group('202', 1)
                        #SSR.running_action_group('82', 1)
                        time.sleep(0.2)
                    elif -3 <= angle_bottom <= 3:
                        #SSR.running_action_group('00', 1)
                        #print(angle_bottom)
                        if bottom_center_x < 0.45*r_w:
                            SSR.running_action_group('160', 1)
                        elif bottom_center_x > 0.55*r_w:
                            SSR.running_action_group('161', 1)
                        else:
                            PWMServo.setServo(1, 1810, 500)
                            PWMServo.setServo(2, 1510, 500)
                            SSR.running_action_group('239', 5)
                            SSR.running_action_group('157', 1)
                            step = 1
                            cv2.destroyAllWindows()
                            
        elif step >= 1:
            t1 = cv2.getTickCount()
            border = cv2.copyMakeBorder(org_img, 8, 8, 6, 6, borderType=cv2.BORDER_CONSTANT,
                                        value=(255, 255, 255))  # 扩展bai边，防止边界无法识别
            org_img_copy = cv2.resize(border, (r_w, r_h), interpolation=cv2.INTER_CUBIC)  # 将图片缩放
            frame_gauss = cv2.GaussianBlur(org_img_copy, (3, 3), 0)  # 高斯模糊
            frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
            frame = cv2.inRange(frame_hsv, color_range['black_hole'][0],
                                color_range['black_hole'][1])  # 对原图像和掩模(颜色的字典)进行位运算
            # cv2.imshow('frame', frame)  # 显示图像
            opened = cv2.morphologyEx(frame, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))  # 开运算 去噪点
            closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8))  # 闭运算 封闭连接

            (image, contours, hierarchy) = cv2.findContours(closed, cv2.RETR_LIST,
                                                            cv2.CHAIN_APPROX_NONE)  # 找出轮廓cv2.CHAIN_APPROX_NONE
            if debug:
                cv2.imshow('closed', closed)  # 显示图像
                cv2.drawContours(org_img_copy, contours, -1, (255, 0, 255), 3)
            areaMaxContour, area_max = getAreaMaxContour(contours)  # 找出最大轮廓
            # print(area_max)
            if areaMaxContour is not None:
                top_right = areaMaxContour[0][0]
                top_left = areaMaxContour[0][0]
                for c in areaMaxContour:
                    if (c[0][0] + 1.3 * c[0][1]) < (top_left[0] + 1.3 * top_left[1]):
                        top_left = c[0]
                    if ((r_w - c[0][0]) + 1.3 * c[0][1]) < ((r_w - top_right[0]) + 1.3 * top_right[1]):
                        top_right = c[0]
                angle = - math.atan((top_right[1] - top_left[1]) / (top_right[0] - top_left[0])) * 180.0 / math.pi
                center_x = (top_right[0] + top_left[0]) / 2
                center_y = (top_right[1] + top_left[1]) / 2
                rect = cv2.minAreaRect(areaMaxContour)
                # center, w_h, angle = rect  # 中心点 宽高 旋转角度
                box = np.int0(cv2.boxPoints(rect))  # 点的坐标
                if debug:
                    cv2.circle(org_img_copy, (top_right[0], top_right[1]), 5, [0, 255, 255], 2)
                    cv2.circle(org_img_copy, (top_left[0], top_left[1]), 5, [0, 255, 255], 2)
                    cv2.drawContours(org_img_copy, [box], 0, (0, 0, 255), 2)  # 将大矩形画在图上
            else:
                angle = 0
                center_x = 0.5*r_w
                center_y = 0
            if debug:
                t2 = cv2.getTickCount()
                time_r = (t2 - t1) / cv2.getTickFrequency()
                fps = 1.0 / time_r
                cv2.putText(org_img_copy, "step:" + str(step), (30, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                            2)  # (0, 0, 255)BGR
                cv2.putText(org_img_copy, "fps:" + str(int(fps)), (30, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                            2)  # (0, 0, 255)BGR
                cv2.putText(org_img_copy, "angle:" + str(int(angle)), (30, 300), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0),
                            2)  # (0, 0, 255)BGR
                cv2.putText(org_img_copy, "center_x:" + str(int(center_x)), (30, 400), cv2.FONT_HERSHEY_SIMPLEX, 0.65,
                            (0, 0, 0), 2)  # (0, 0, 255)BGR
                cv2.putText(org_img_copy, "center_y:" + str(int(center_y)), (330, 400), cv2.FONT_HERSHEY_SIMPLEX, 0.65,
                            (0, 0, 0), 2)  # (0, 0, 255)BGR
                cv2.imshow('org_img_copy', org_img_copy)  # 显示图像
                cv2.waitKey(1)
            # print('angle: %f' %(angle))
            # print(angle)
            # print('center_x: %f' %(center_x))
            # print('center_y: %f' %(center_y))
            if step == 1:  # 看黑色上顶点，调整方向和位置
                if angle < -1.5:  # 右转
                    #SSR.running_action_group('84', 1)
                    SSR.running_action_group('203', 1)
                    time.sleep(0.1)
                elif angle > 3:  # 左转
                    #SSR.running_action_group('82', 1)
                    SSR.running_action_group('202', 1)
                    time.sleep(0.1)
                elif -1.5 <= angle <= 3:  
                    if center_x > 0.54 * r_w:
                        SSR.running_action_group('161', 1)
                        time.sleep(0.1)
                    elif center_x < 0.48 * r_w:  # 左移
                        SSR.running_action_group('160', 1)
                        time.sleep(0.1)
                    elif 0.48 * r_w <= center_x <= 0.54 * r_w:
                        step = 2
            elif step == 2:
                if center_y < 0.58 * r_h:  # 小步挪前进
                    SSR.running_action_group('232', 1)
                elif 0.58 * r_h <= center_y <= 0.7 * r_h:
                    step = 3
                elif center_y > 0.7 * r_h:  # 后退
                    SSR.running_action_group('181', 2)
            elif step == 3:
                    if angle < -1.5:  # 右转
                        #SSR.running_action_group('84', 1)
                        SSR.running_action_group('203', 1)
                        time.sleep(0.1)
                    elif angle > 3:  # 左转
                        #SSR.running_action_group('82', 1)
                        SSR.running_action_group('202', 1)
                        time.sleep(0.1)
                    elif -1.5 <= angle <= 3:
                        if center_x > 0.54 * r_w:
                            SSR.running_action_group('161', 1)
                            time.sleep(0.1)
                        elif center_x < 0.48 * r_w:  # 左移
                            SSR.running_action_group('160', 1)
                            time.sleep(0.1)
                        elif 0.48 * r_w <= center_x <= 0.54 * r_w:
                            PWMServo.setServo(1, 2200, 500)
                            PWMServo.setServo(2, 1510, 500)
                            SSR.running_action_group('265', 1)
                            SSR.running_action_group('239', 2)
                            SSR.running_action_group('157', 1)
                            cv2.destroyAllWindows()
                            state_sel = None
                            state += 1
                            step = -1
                            break
                                       





#################################################第九关：沟壑###############################################
def gap(r_w, r_h):
    global org_img, state, step, reset, skip, debug
    if state == 9:
        print('进入gap')
        step = -1
        PWMServo.setServo(1, 2100, 500)  # NO3机器人，初始云台角度
        PWMServo.setServo(2, 1510, 500)
        SSR.running_action_group('00', 1)
    else:
        return
    while state == 9:
        if reset == 1:  # 是否为重置情况
            # PWMServo.setServo(1, 2100, 500) # NO2机器人，初始云台角度
            # PWMServo.setServo(2, 1510, 500)
            PWMServo.setServo(1, 2100, 500)  # NO3机器人，初始云台角度
            PWMServo.setServo(2, 1510, 500)
            step = 0
            reset = 0
            SSR.running_action_group('0', 1)
        if skip == 1:  # 跳至下一关
            cv2.destroyAllWindows()
            state += 1
            skip = 0
            state_sel = None
            SSR.running_action_group('666', 1)
            time.sleep(5)
            break
        if step == -1:
            direction(640, 480, 3)
            step = 0
            PWMServo.setServo(1, 2100, 500)  # NO3机器人，初始云台角度
            PWMServo.setServo(2, 1510, 500)
            SSR.running_action_group('00', 1)
            time.sleep(0.2)
            continue
        
        t1 = cv2.getTickCount()
        # blobs = org_img[int(0.2 * ori_h):int(0.8 * ori_h), int(0.2 * ori_w):int(0.8 * ori_w)]  # 只对中间部分识别处理
        border = cv2.copyMakeBorder(org_img, 8, 8, 6, 6, borderType=cv2.BORDER_CONSTANT,
                                    value=(255, 255, 255))  # 扩展白边，防止边界无法识别
        org_img_copy = cv2.resize(border, (r_w, r_h), interpolation=cv2.INTER_CUBIC)  # 将图片缩放
        frame_gauss = cv2.GaussianBlur(org_img_copy, (3, 3), 0)  # 高斯模糊
        frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        frame = cv2.inRange(frame_hsv, color_range['black_gap'][0],
                                  color_range['black_gap'][1])  # 对原图像和掩模(颜色的字典)进行位运算
        opened = cv2.morphologyEx(frame, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))  # 开运算 去噪点
        closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8))  # 闭运算 封闭连接

        # print(closed)
        (image, contours, hierarchy) = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)  # 找出轮廓
        if debug:
            cv2.imshow('closed', closed)  # 显示图像
            cv2.drawContours(org_img_copy, contours, -1, (255, 0, 255), 2)
        #areaMaxContour, area_max = getAreaMaxContour(contours)  # 找出最大轮廓
        areaSumContour = getSumContour(contours, area=25)
        area_sum = getAreaSumContour(contours)
        percent = round(100 * area_sum / (r_w * r_h), 2)
        if areaSumContour is not None:
            rect = cv2.minAreaRect(areaSumContour)
            #center, w_h, angle = rect  # 中心点 宽高 旋转角度
            box = np.int0(cv2.boxPoints(rect))  # 点的坐标
            cv2.drawContours(org_img_copy, [box], 0, (0, 0, 255), 2)  # 将大矩形画在图上
            top_left = areaSumContour[0][0]
            top_right = areaSumContour[0][0]
            bottom_left = areaSumContour[0][0]
            bottom_right = areaSumContour[0][0]
            for c in areaSumContour:  # 遍历找到四个顶点
                if c[0][0] + 1 * c[0][1] < top_left[0] + 1 * top_left[1]:
                    top_left = c[0]
                if (r_w - c[0][0]) + 1 * c[0][1] < (r_w - top_right[0]) + 1 * top_right[1]:
                    top_right = c[0]
                if c[0][0] + 1 * (r_h - c[0][1]) < bottom_left[0] + 1 * (r_h - bottom_left[1]):
                    bottom_left = c[0]
                if c[0][0] + 1 * c[0][1] > bottom_right[0] + 1 * bottom_right[1]:
                    bottom_right = c[0]
            angle_top = - math.atan((top_right[1] - top_left[1]) / (top_right[0] - top_left[0])) * 180.0 / math.pi
            angle_bottom = - math.atan((bottom_right[1] - bottom_left[1]) / (bottom_right[0] - bottom_left[0])) * 180.0 / math.pi
            #top_center_x = int((top_right[0] + top_left[0]) / 2)
            top_center_y = int((top_right[1] + top_left[1]) / 2)
            #bottom_center_x = int((bottom_right[0] + bottom_left[0]) / 2)
            bottom_center_y = int((bottom_right[1] + bottom_left[1]) / 2)
            if debug:
                cv2.circle(org_img_copy, (top_right[0], top_right[1]), 5, [0, 255, 255], 2)
                cv2.circle(org_img_copy, (top_left[0], top_left[1]), 5, [0, 255, 255], 2)
                cv2.circle(org_img_copy, (bottom_right[0], bottom_right[1]), 5, [0, 255, 255], 2)
                cv2.circle(org_img_copy, (bottom_left[0], bottom_left[1]), 5, [0, 255, 255], 2)
        else:
            angle_top = 0
            angle_bottom = 0
            top_center_y = 0
            bottom_center_y = 0
        # 求得大矩形的旋转角度，if条件是为了判断长的一条边的旋转角度，因为box存储的点的顺序不确定
        #if math.sqrt(math.pow(box[3, 1] - box[0, 1], 2) + math.pow(box[3, 0] - box[0, 0], 2)) > math.sqrt(
        #        math.pow(box[3, 1] - box[2, 1], 2) + math.pow(box[3, 0] - box[2, 0], 2)):
        #    if (box[3, 0] - box[0, 0]) > 1:
        #        angle = - math.atan((box[3, 1] - box[0, 1]) / (box[3, 0] - box[0, 0])) * 180.0 / math.pi
        #    else:
        #        angle = 90
        #else:
        #    if (box[3, 0] - box[2, 0]) > 1:
        #        angle = - math.atan((box[3, 1] - box[2, 1]) / (box[3, 0] - box[2, 0])) * 180.0 / math.pi  # 负号是因为坐标原点的问题
        #    else:
        #        angle = 90
        if debug:
            cv2.putText(org_img_copy, 'area: ' + str(percent) + '%', (30, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255),
                        2)
            cv2.putText(org_img_copy, 'angle_top: ' + str(round(angle_top, 2)), (30, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
                        (0, 0, 255), 2)
            cv2.putText(org_img_copy, 'angle_bottom: ' + str(round(angle_bottom, 2)), (30, 300), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
                        (0, 0, 255), 2)

            cv2.putText(org_img_copy, 'top_y: ' + str(round(top_center_y, 2)), (330, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
                        (0, 0, 255), 2)  # if center
            cv2.putText(org_img_copy, 'bottom_y: ' + str(round(bottom_center_y, 2)), (330, 300), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
                        (0, 0, 255), 2)  # if center
            t2 = cv2.getTickCount()
            time_r = (t2 - t1) / cv2.getTickFrequency()
            fps = 1.0 / time_r
            #print(fps)
            cv2.putText(org_img_copy, "fps:" + str(int(fps)), (30, 400), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255),
                        2)  # (0, 0, 255)BGR
            cv2.imshow('org_img_copy', org_img_copy)  # 显示图像
            cv2.waitKey(1)
        if step == 0:
            if bottom_center_y < 0.5*r_h:#接近沟壑
                SSR.running_action_group('239', 1)
            else:
                SSR.running_action_group('157', 1)
                step = 1
        if step == 1: #看下顶点 小步挪
            if bottom_center_y < 0.7*r_h:
                SSR.running_action_group('232', 1)
            elif bottom_center_y >= 0.7*r_h:
                PWMServo.setServo(1, 2000, 500)  # NO3机器人，初始云台角度
                PWMServo.setServo(2, 1510, 500)
                time.sleep(0.5)
                step = 2       
        elif step == 2:#看上顶点
            if angle_top > 3:
                SSR.running_action_group('202', 1)#左转
                #time.sleep(0.2)
            elif angle_top < -3:
                SSR.running_action_group('203', 1)#右转
                #time.sleep(0.2)
            elif -3 <= angle_top <= 3:
                step = 3
        elif step == 3:
            if top_center_y > 0.6 * r_h:
                PWMServo.setServo(1, 2240, 500)
                PWMServo.setServo(2, 1510, 500)
                time.sleep(0.2)
                SSR.running_action_group('229', 1)#跨沟壑
                cv2.destroyAllWindows()
                step = 4
            else:
                SSR.running_action_group('232', 1) #更小步挪
        
        elif step == 4:#调整方向走五步，靠近终点
            #direction(640, 480, 2)
            SSR.running_action_group('239', 2)
            SSR.running_action_group('157', 1)
            state = 10
            step = 0
            break
  


#################################################第十关：终点###############################################
def end_door(r_w, r_h):
    global org_img, state, reset, debug, Running, step, state_sel
    if state == 10:  # 初始化
        print('进入end_door')
        step = 0
        PWMServo.setServo(1, 1700, 500)
        PWMServo.setServo(2, 1510, 500)
        SSR.running_action_group('00', 1)
    else:
        return
    while state == 10:
        if reset == 1:  # 是否为重置情况
            # PWMServo.setServo(1, 2100, 500) # NO2机器人，初始云台角度
            # PWMServo.setServo(2, 1510, 500)
            PWMServo.setServo(1, 1700, 500)  # NO3机器人，初始云台角度
            PWMServo.setServo(2, 1510, 500)
            reset = 0
            step = 0
            cv2.destroyAllWindows()
            SSR.running_action_group('0', 1)
        #t1 = cv2.getTickCount()
        border = cv2.copyMakeBorder(org_img, 12, 12, 16, 16, borderType=cv2.BORDER_CONSTANT,
                                    value=(255, 255, 255))  # 扩展黑边，防止边界无法识别
        org_img_copy = cv2.resize(border, (r_w, r_h), interpolation=cv2.INTER_CUBIC)  # 将图片缩放
        frame_gauss = cv2.GaussianBlur(org_img_copy, (3, 3), 0)  # 高斯模糊
        frame_hsv = cv2.cvtColor(frame_gauss, cv2.COLOR_BGR2HSV)  # 将图片转换到HSV空间
        frame_yellow = cv2.inRange(frame_hsv, color_range['yellow_door'][0],
                                   color_range['yellow_door'][1])  # 对原图像和掩模(颜色的字典)进行位运算
        #frame_yellow2 = cv2.inRange(frame_hsv, color_range['black_door'][0],
                                 #color_range['black_door'][1])  # 对原图像和掩模(颜色的字典)进行位运算
        #frame_yellow = cv2.add(frame_yellow1, frame_yellow2)
        opened = cv2.morphologyEx(frame_yellow, cv2.MORPH_OPEN, np.ones((3, 3), np.uint8))  # 开运算 去噪点
        closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8))  # 闭运算 封闭连接

        (image, contours, hierarchy) = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)  # 找出轮廓
        #areaMaxContour, area_max = getAreaMaxContour(contours)  # 找出最大轮廓
        area_max = getAreaSumContour(contours)
        percent = round(100 * area_max / (r_w * r_h), 2)
        cv2.putText(org_img, 'area: ' + str(percent), (30, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        if debug:
            cv2.imshow('closed', closed)  # 显示图像
            cv2.imshow('org_img', org_img)  # 显示图像
            cv2.waitKey(1)
        if percent < 3 and step == 0:
            time.sleep(0.01)
        elif percent >= 5 and step == 0:
            step = 1
            time.sleep(1)
        elif percent >= 5  and step == 1:
            time.sleep(0.01)
        elif percent < 3 and step == 1:
            SSR.running_action_group('239', 20)
            state = 1
            step = 0
            state_sel = None
            Running = False
            break
        else:
            time.sleep(0.01)




####################################################主循环##################################################
while True:  # 每一帧进行循环处理
    if org_img is not None and ret:
        if Running:
            start_door(320, 240)
            floor(320, 240)  # 640*480 fps=3   160*120 fps=15
            obscle()
            baffle(320, 240)
            door(320, 240)
            bridge(320, 240)
            kick_ball(640,480)
            square_hole(640, 480)
            gap(320, 240)  #640*480 fps=1~2  320*240 fps=4~30
            end_door(320, 240)
            #direction(640,480,2)
        else:
            print('Running is false')
            time.sleep(0.01)
    else:
        print('image is empty')
        time.sleep(0.01)
        # cv2.destroyAllWindows()